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 : 57710 : void Reset() {
60 : 57710 : x1 = x2 = y1 = y2 = 0.0;
61 : 57710 : }
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 : 55565 : inline double inverse_lerp(double min_val, double max_val, double value) {
69 : 55565 : double range = max_val - min_val;
70 [ + + ]: 55565 : if (std::abs(range) >= 0.0001) {
71 : : // NOTE: The ternary below is redundant but kept for safety.
72 [ + - ]: 55557 : double t = (value - min_val) / (std::abs(range) >= 0.0001 ? range : 1.0);
73 : 55557 : 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 : 114626 : inline double smoothstep(double edge0, double edge1, double x) {
86 : 114626 : double range = edge1 - edge0;
87 [ + + ]: 114626 : if (std::abs(range) >= 0.0001) {
88 : : // NOTE: The ternary below is redundant but kept for safety.
89 [ + - ]: 114622 : double t = (x - edge0) / (std::abs(range) >= 0.0001 ? range : 1.0);
90 : 114622 : t = (std::max)(0.0, (std::min)(1.0, t));
91 : 114622 : 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 : 55380 : inline double apply_slew_limiter(double input, double& prev_val, double limit, double dt) {
99 : 55380 : double delta = input - prev_val;
100 : 55380 : double max_change = limit * dt;
101 : 55380 : delta = std::clamp(delta, -max_change, max_change);
102 : 55380 : prev_val += delta;
103 : 55380 : 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 : 118689 : inline double apply_adaptive_smoothing(double input, double& prev_out, double dt,
109 : : double slow_tau, double fast_tau, double sensitivity) {
110 : 118689 : double delta = std::abs(input - prev_out);
111 : 118689 : double t = delta / (sensitivity + 0.000001);
112 : 118689 : t = (std::min)(1.0, t);
113 : :
114 : 118689 : double tau = slow_tau + t * (fast_tau - slow_tau);
115 : 118689 : double alpha = dt / (tau + dt + 1e-9);
116 : 118689 : alpha = (std::max)(0.0, (std::min)(1.0, alpha));
117 : :
118 : 118689 : prev_out = prev_out + alpha * (input - prev_out);
119 : 118689 : 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 : 31 : inline double apply_load_transform_cubic(double x) {
127 : 31 : 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 : 10 : inline double apply_load_transform_quadratic(double x) {
135 : 10 : 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 : 10 : inline double apply_load_transform_hermite(double x) {
143 : 10 : double abs_x = std::abs(x);
144 : 10 : 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 : 219935 : 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 [ + + ]: 219935 : if (buffer_count < window) return 0.0;
156 : :
157 : 209438 : int M = window / 2; // Half-width (e.g., window=15 -> M=7)
158 : :
159 : : // Calculate S_2 = M(M+1)(2M+1)/3
160 : 209438 : 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 : 209438 : int latest_idx = (buffer_index - 1 + BufferSize) % BufferSize;
167 : 209438 : int center_idx = (latest_idx - M + BufferSize) % BufferSize;
168 : :
169 : 209438 : double sum = 0.0;
170 [ + + ]: 1661041 : for (int k = 1; k <= M; ++k) {
171 : 1451603 : int idx_pos = (center_idx + k + BufferSize) % BufferSize;
172 : 1451603 : int idx_neg = (center_idx - k + BufferSize) % BufferSize;
173 : :
174 : : // Weights for d=1 are simply k
175 : 1451603 : sum += (double)k * (buffer[idx_pos] - buffer[idx_neg]);
176 : : }
177 : :
178 : : // Divide by dt to get derivative in units/second
179 : 209438 : return sum / (S2 * dt);
180 : : }
181 : :
182 : : /**
183 : : * @brief Linear Interpolator (Inter-Frame Reconstruction)
184 : : *
185 : : * Upsamples a 100Hz signal to 400Hz+ by delaying the signal by 1 frame (10ms)
186 : : * and smoothly interpolating. Eliminates sawtooth artifacts and derivative spikes.
187 : : */
188 : : class LinearExtrapolator { // Kept name to avoid breaking other files
189 : : private:
190 : : double m_prev_sample = 0.0;
191 : : double m_target_sample = 0.0;
192 : : double m_current_output = 0.0;
193 : : double m_rate = 0.0;
194 : : double m_time_since_update = 0.0;
195 : : double m_game_tick = 0.01; // Default 100Hz
196 : : bool m_initialized = false;
197 : :
198 : : public:
199 : 2 : void Configure(double game_tick) {
200 : 2 : m_game_tick = (std::max)(0.0001, game_tick);
201 : 2 : }
202 : :
203 : 1893965 : double Process(double raw_input, double dt, bool is_new_frame) {
204 [ + + ]: 1893965 : if (!m_initialized) {
205 : 12898 : m_prev_sample = raw_input;
206 : 12898 : m_target_sample = raw_input;
207 : 12898 : m_current_output = raw_input;
208 : 12898 : m_initialized = true;
209 : 12898 : return raw_input;
210 : : }
211 : :
212 [ + + ]: 1881067 : if (is_new_frame) {
213 : : // Shift the window: old target becomes the new start point
214 : 864739 : m_prev_sample = m_target_sample;
215 : 864739 : m_target_sample = raw_input;
216 : :
217 : : // Calculate rate to reach the new target over the next game tick
218 : 864739 : m_rate = (m_target_sample - m_prev_sample) / m_game_tick;
219 : 864739 : m_time_since_update = 0.0;
220 : :
221 : : // Output starts exactly at the previous sample (no snapping)
222 : 864739 : m_current_output = m_prev_sample;
223 : : } else {
224 : 1016328 : m_time_since_update += dt;
225 [ + + ]: 1016328 : if (m_time_since_update <= m_game_tick) {
226 : 1016136 : m_current_output = m_prev_sample + m_rate * m_time_since_update;
227 : : } else {
228 : : // If the game stutters/drops a frame, hold the target value safely
229 : 192 : m_current_output = m_target_sample;
230 : : }
231 : : }
232 : :
233 : 1881067 : return m_current_output;
234 : : }
235 : :
236 : 12672 : void Reset() {
237 : 12672 : m_prev_sample = m_target_sample = m_current_output = m_rate = m_time_since_update = 0.0;
238 : 12672 : m_initialized = false;
239 : 12672 : }
240 : : };
241 : :
242 : : /**
243 : : * @brief Second-Order Holt-Winters (Double Exponential Smoothing)
244 : : *
245 : : * Tracks both the value and the trend (velocity) of a signal.
246 : : * Acts as both an upsampler and a low-pass filter.
247 : : */
248 : : class HoltWintersFilter {
249 : : private:
250 : : double m_level = 0.0; // Smoothed value (Target for interpolation)
251 : : double m_prev_level = 0.0; // Previous smoothed value (Start for interpolation)
252 : : double m_trend = 0.0; // Smoothed trend/slope
253 : : double m_time_since_update = 0.0;
254 : : bool m_initialized = false;
255 : : bool m_zero_latency = true; // Mode toggle
256 : :
257 : : // Tuning Parameters
258 : : double m_alpha = 0.8; // Level weight (Higher = less lag)
259 : : double m_beta = 0.2; // Trend weight
260 : : double m_game_tick = 0.01; // Default 100Hz
261 : :
262 : : public:
263 : 1 : void Configure(double alpha, double beta, double game_tick = 0.01) {
264 : 1 : m_alpha = std::clamp(alpha, 0.01, 1.0);
265 : 1 : m_beta = std::clamp(beta, 0.01, 1.0);
266 : 1 : m_game_tick = (std::max)(0.0001, game_tick);
267 : 1 : }
268 : :
269 : 59186 : void SetZeroLatency(bool zero_latency) {
270 : 59186 : m_zero_latency = zero_latency;
271 : 59186 : }
272 : :
273 : 59188 : double Process(double raw_input, double dt, bool is_new_frame) {
274 [ + + ]: 59188 : if (!m_initialized) {
275 : 404 : m_level = raw_input;
276 : 404 : m_prev_level = raw_input;
277 : 404 : m_trend = 0.0;
278 : 404 : m_time_since_update = 0.0;
279 : 404 : m_initialized = true;
280 : 404 : return raw_input;
281 : : }
282 : :
283 [ + + ]: 58784 : if (is_new_frame) {
284 : 27024 : double old_level = m_level;
285 : 27024 : m_prev_level = m_level; // Save for interpolation start point
286 : :
287 : : // Update Level: Balance between the raw input and our previous prediction
288 : 27024 : m_level = m_alpha * raw_input + (1.0 - m_alpha) * (m_level + m_trend * m_game_tick);
289 : :
290 : : // Update Trend: Balance between the new observed slope and the old trend
291 : 27024 : m_trend = m_beta * ((m_level - old_level) / m_game_tick) + (1.0 - m_beta) * m_trend;
292 : :
293 : 27024 : m_time_since_update = 0.0;
294 : :
295 [ + + ]: 27024 : if (m_zero_latency) {
296 : 27023 : return m_level;
297 : : } else {
298 : 1 : return m_prev_level; // Start interpolation from previous level
299 : : }
300 : : } else {
301 : 31760 : m_time_since_update += dt;
302 : : }
303 : :
304 [ + + ]: 31760 : if (m_zero_latency) {
305 : : // Predict current state based on previous trend (Extrapolation)
306 : 31758 : return m_level + m_trend * m_time_since_update;
307 : : } else {
308 : : // Smoothly blend between prev_level and level (Interpolation)
309 [ + - ]: 2 : if (m_time_since_update <= m_game_tick) {
310 : 2 : double t = m_time_since_update / m_game_tick;
311 : 2 : return m_prev_level + t * (m_level - m_prev_level);
312 : : } else {
313 : 0 : return m_level; // Hold target if game stutters
314 : : }
315 : : }
316 : : }
317 : :
318 : 396 : void Reset() {
319 : 396 : m_level = m_prev_level = m_trend = m_time_since_update = 0.0;
320 : 396 : m_initialized = false;
321 : 396 : }
322 : : };
323 : :
324 : : } // namespace ffb_math
325 : :
326 : : #endif // MATH_UTILS_H
|
