Source code for suboptimumg.track.gps

from typing import Any, Tuple

import numpy as np
import numpy.typing as npt
from scipy.interpolate import splev, splprep




def resample_xy_uniform(
    x_m: npt.NDArray[np.float64],
    y_m: npt.NDArray[np.float64],
    distance_step: float,
    smoothing: float = 0.0,
) -> Tuple[
    npt.NDArray[np.float64], npt.NDArray[np.float64], npt.NDArray[np.float64], Any
]:
    """
    Fit a B-spline through Cartesian XY coordinates and resample at a uniform
    arc-length spacing.

    The spline is fit in XY space (not lat/lon) so the resulting points are
    genuinely equally spaced in meters.

    Parameters
    ----------
    x_m : NDArray[float64]
        East coordinates in meters (clean, no NaNs)
    y_m : NDArray[float64]
        North coordinates in meters (clean, no NaNs)
    distance_step : float
        Desired spacing between output points in meters
    smoothing : float
        Spline smoothing factor (0 = interpolating spline)

    Returns
    -------
    NDArray[float64]
        x_uniform - Uniformly-spaced East coordinates in meters
    NDArray[float64]
        y_uniform - Uniformly-spaced North coordinates in meters
    NDArray[float64]
        cumulative_dist - Arc-length distances at each output point (meters)
    Any
        tck - Scipy B-spline representation for later evaluation
    """
    x_m = np.asarray(x_m, dtype=np.float64)
    y_m = np.asarray(y_m, dtype=np.float64)

    # Remove consecutive duplicates (splprep requires distinct points)
    dx = np.diff(x_m, prepend=np.nan)
    dy = np.diff(y_m, prepend=np.nan)
    step = np.sqrt(dx**2 + dy**2)
    unique = np.concatenate(([True], step[1:] > 0))
    x_m = x_m[unique]
    y_m = y_m[unique]

    # Compute arc-length parameterisation from the cleaned points
    seg_len = np.sqrt(np.diff(x_m) ** 2 + np.diff(y_m) ** 2)
    arc_length = np.zeros(len(x_m))
    arc_length[1:] = np.cumsum(seg_len)

    total_length = arc_length[-1]
    tck, _ = splprep([x_m, y_m], u=arc_length, s=smoothing, k=3)

    num_points = int(np.ceil(total_length / distance_step)) + 1
    cumulative_dist = np.linspace(0.0, total_length, num_points)

    x_uniform, y_uniform = splev(cumulative_dist, tck)

    return (
        np.asarray(x_uniform, dtype=np.float64),
        np.asarray(y_uniform, dtype=np.float64),
        cumulative_dist,
        tck,
    )