utils.math

75def default_segment_points() -> list[dict]:
76    """Generate default 3-point linear control points (y = x)."""
77    return [
78        {"x": -1.0, "y": -1.0},
79        {"x": 0.0, "y": 0.0},
80        {"x": 1.0, "y": 1.0},
81    ]

37def default_spline_points() -> list[dict]:
38    """Generate default 3-point linear control points (y = x)."""
39    return [
40        {"x": -1.0, "y": -1.0, "tangent": 1.0},
41        {"x": 0.0, "y": 0.0, "tangent": 1.0},
42        {"x": 1.0, "y": 1.0, "tangent": 1.0},
43    ]

 84def evaluate_segments(points: list[dict], x: float) -> float:
 85    """Evaluate the piecewise-linear curve at input *x*.
 86
 87    Linearly interpolates between adjacent control points.
 88    """
 89    if not points or len(points) < 2:
 90        return x
 91    x = max(points[0]["x"], min(points[-1]["x"], x))
 92    for i in range(len(points) - 1):
 93        x0, x1 = points[i]["x"], points[i + 1]["x"]
 94        if x <= x1 or i == len(points) - 2:
 95            dx = x1 - x0
 96            if dx == 0:
 97                return points[i]["y"]
 98            t = (x - x0) / dx
 99            y0, y1 = points[i]["y"], points[i + 1]["y"]
100            return y0 + t * (y1 - y0)
101    return x

46def evaluate_spline(points: list[dict], x: float) -> float:
47    """Evaluate the cubic hermite spline at input *x*, returning output *y*.
48
49    Mathematically identical to wpimath.spline.CubicHermiteSpline.
50    """
51    if not points or len(points) < 2:
52        return x
53    x = max(points[0]["x"], min(points[-1]["x"], x))
54    for i in range(len(points) - 1):
55        x0, x1 = points[i]["x"], points[i + 1]["x"]
56        if x <= x1 or i == len(points) - 2:
57            dx = x1 - x0
58            if dx == 0:
59                return points[i]["y"]
60            t = (x - x0) / dx
61            return hermite_eval(
62                points[i]["y"], points[i]["tangent"],
63                points[i + 1]["y"], points[i + 1]["tangent"],
64                dx, t)
65    return x

19def hermite_eval(y0, m0, y1, m1, dx, t):
20    """Evaluate one cubic hermite segment at parameter *t* in [0, 1].
21
22    Args:
23        y0, y1: endpoint Y values
24        m0, m1: endpoint slopes (dy/dx)
25        dx: segment width (x1 - x0)
26        t: parameter in [0, 1]
27    """
28    t2 = t * t
29    t3 = t2 * t
30    h00 = 2 * t3 - 3 * t2 + 1
31    h10 = t3 - 2 * t2 + t
32    h01 = -2 * t3 + 3 * t2
33    h11 = t3 - t2
34    return h00 * y0 + h10 * dx * m0 + h01 * y1 + h11 * dx * m1

68def numerical_slope(points: list[dict], x: float) -> float:
69    """Estimate dy/dx at *x* by central difference."""
70    eps = 0.001
71    return (evaluate_spline(points, x + eps)
72            - evaluate_spline(points, x - eps)) / (2 * eps)