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 : 10864 : void Reset() {
60 : 10864 : x1 = x2 = y1 = y2 = 0.0;
61 : 10864 : }
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 : 15193 : inline double smoothstep(double edge0, double edge1, double x) {
86 : 15193 : double range = edge1 - edge0;
87 [ + + ]: 15193 : if (std::abs(range) >= 0.0001) {
88 : : // NOTE: The ternary below is redundant but kept for safety.
89 [ + - ]: 15189 : double t = (x - edge0) / (std::abs(range) >= 0.0001 ? range : 1.0);
90 : 15189 : t = (std::max)(0.0, (std::min)(1.0, t));
91 : 15189 : 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 : 24708 : inline double apply_adaptive_smoothing(double input, double& prev_out, double dt,
109 : : double slow_tau, double fast_tau, double sensitivity) {
110 : 24708 : double delta = std::abs(input - prev_out);
111 : 24708 : double t = delta / (sensitivity + 0.000001);
112 : 24708 : t = (std::min)(1.0, t);
113 : :
114 : 24708 : double tau = slow_tau + t * (fast_tau - slow_tau);
115 : 24708 : double alpha = dt / (tau + dt + 1e-9);
116 : 24708 : alpha = (std::max)(0.0, (std::min)(1.0, alpha));
117 : :
118 : 24708 : prev_out = prev_out + alpha * (input - prev_out);
119 : 24708 : 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 : : } // namespace ffb_math
157 : :
158 : : #endif // MATH_UTILS_H
|
