1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
#[derive(Clone, Copy, Debug)]
struct LinearPiece {
pub base_x: f32,
pub base_y: f32,
pub max_x: f32,
pub max_y: f32,
pub x_diff: f32,
pub y_diff: f32,
}
/// Continuous piecewise-linear `(x, y)` mapping (e.g. steer vs speed).
///
/// Lookup clamps: inputs at/below the first `x` return the first `y`, inputs
/// above the last `x` return the last `y`. Used throughout `consts::curves`.
#[derive(Clone, Copy, Debug)]
pub struct LinearPieceCurve<const N: usize> {
curve: [LinearPiece; N],
}
impl<const N: usize> LinearPieceCurve<N> {
/// Builds a curve from ascending `(x, y)` control points.
///
/// `x` values should ascend; behavior is linear interpolation between
/// neighbors (no validation in `const` context).
pub const fn new(value_mappings: [(f32, f32); N]) -> Self {
let mut curve = [LinearPiece {
base_x: 0.0,
base_y: 0.0,
max_x: 0.0,
max_y: 0.0,
x_diff: 0.0,
y_diff: 0.0,
}; N];
curve[0].max_x = value_mappings[0].0;
curve[0].max_y = value_mappings[0].1;
let mut i = 1;
while i < N {
let prev = &value_mappings[i - 1];
let this = &value_mappings[i];
curve[i].base_x = prev.0;
curve[i].base_y = prev.1;
curve[i].max_x = this.0;
curve[i].max_y = this.1;
curve[i].x_diff = this.0 - prev.0;
curve[i].y_diff = this.1 - prev.1;
i += 1;
}
Self { curve }
}
/// Samples the curve at `input` (clamped at both ends, see type docs).
///
/// # Arguments
///
/// * `input` - The input to the curve
#[inline]
pub fn get_output(&self, input: f32) -> f32 {
debug_assert!(N != 0);
// A single-point curve is constant: both clamp ends return its `y`.
if N == 1 {
return self.curve[0].max_y;
}
let first_val_pair = self.curve[0];
if input <= first_val_pair.max_x {
return first_val_pair.max_y;
}
// Manual index loop over `1..N`: the same strict-`>` first-match
// selection as the former `skip(1).find(...)`, but without iterator
// adapters. `N` is const per monomorphization, so this unrolls into
// check-free indexed loads.
let mut i = 1usize;
while i < N {
if self.curve[i].max_x > input {
break;
}
i += 1;
}
if i == N {
return self.curve[N - 1].max_y;
}
let pair = self.curve[i];
let interp_frac = (input - pair.base_x) / pair.x_diff;
pair.y_diff * interp_frac + pair.base_y
}
}