crgutils 0.1.0__py3-none-any.whl

crgutils/__init__.py

@@ -0,0 +1,38 @@
+"""crgutils — Python library for reading and evaluating OpenCRG road surface files.
+
+Quick start
+-----------
+>>> import crgutils
+>>> ds = crgutils.read("road.crg")         # load + validate + apply modifiers
+>>> cp = ds.create_contact_point()
+>>> z = cp.eval_uv_to_z(10.0, 0.0)        # elevation at (u=10, v=0)
+>>> x, y = cp.eval_uv_to_xy(10.0, 0.5)   # road → world coords
+>>> u, v = cp.eval_xy_to_uv(x, y)         # world → road coords
+"""
+
+from ._convenience import read
+from ._dataset import CRGDataset
+from ._contact_point import ContactPoint
+from ._types import BorderMode, RefLineContinue, CurvMode, GridNaNMode, EvalOptions
+
+# Lower-level building blocks — available for advanced use but not part of the
+# primary public API.
+from ._loader import load  # noqa: F401
+from ._check import check  # noqa: F401
+from ._modifiers import apply_modifiers  # noqa: F401
+
+__version__ = "0.1.0"
+
+__all__ = [
+    # High-level interface
+    "read",
+    "CRGDataset",
+    "ContactPoint",
+    # Types / enums
+    "BorderMode",
+    "RefLineContinue",
+    "CurvMode",
+    "GridNaNMode",
+    "EvalOptions",
+    "__version__",
+]

crgutils/_check.py

@@ -0,0 +1,102 @@
+"""Validate a CRGDataset for consistency and accuracy.
+
+Port of ``crgCheck()`` / ``crgCheckOpts()`` / ``crgCheckData()`` from
+crgLoader.c / crgMgr.c.
+"""
+
+from __future__ import annotations
+
+import math
+import warnings
+
+import numpy as np
+
+from ._dataset import CRGDataset
+
+
+def check(dataset: CRGDataset, *, raise_on_error: bool = False) -> bool:
+    """Validate *dataset* for internal consistency.
+
+    Parameters
+    ----------
+    dataset:
+        The dataset to validate.
+    raise_on_error:
+        If True, raise a ``ValueError`` instead of returning False.
+
+    Returns
+    -------
+    bool
+        ``True`` if all checks pass.
+    """
+    errors: list[str] = []
+    warnings_: list[str] = []
+
+    # --- basic shape checks ---
+    if dataset.n_u < 2:
+        errors.append(f"u axis has fewer than 2 points ({dataset.n_u})")
+    if dataset.n_v < 1:
+        errors.append(f"v axis has no points ({dataset.n_v})")
+    if dataset.z.shape != (dataset.n_v, dataset.n_u):
+        errors.append(
+            f"z shape {dataset.z.shape} does not match (n_v={dataset.n_v}, n_u={dataset.n_u})"
+        )
+
+    # --- u increment consistency ---
+    if dataset.n_u >= 2:
+        computed_inc = (dataset.u_max - dataset.u_min) / (dataset.n_u - 1)
+        if abs(computed_inc - dataset.u_inc) > 1e-6 * max(1.0, abs(dataset.u_inc)):
+            warnings_.append(
+                f"u_inc mismatch: stored={dataset.u_inc:.6f}, computed={computed_inc:.6f}"
+            )
+
+    # --- v monotonicity ---
+    if dataset.n_v >= 2:
+        diffs = np.diff(dataset.v.astype(float))
+        if not np.all(diffs > 0):
+            errors.append("v array is not strictly increasing")
+
+    # --- phi continuity (warn on large jumps) ---
+    if dataset.n_u >= 2:
+        phi_unwrapped = np.unwrap(dataset.phi.astype(float))
+        dphi = np.abs(np.diff(phi_unwrapped))
+        max_dphi = float(dphi.max()) if len(dphi) else 0.0
+        if max_dphi > math.pi / 2:
+            warnings_.append(
+                f"Large heading jump detected: max |Δphi| = {math.degrees(max_dphi):.1f}°"
+            )
+
+    # --- NaN fraction in z grid ---
+    nan_count = int(np.isnan(dataset.z.astype(float)).sum())
+    total = dataset.z.size
+    if nan_count > 0:
+        frac = nan_count / total
+        if frac > 0.5:
+            errors.append(f"More than 50% of z values are NaN ({nan_count}/{total})")
+        elif frac > 0.1:
+            warnings_.append(f"{nan_count}/{total} z values are NaN ({100*frac:.1f}%)")
+
+    # --- reference line x/y length vs u length ---
+    if dataset.n_u >= 2:
+        dx = np.diff(dataset.x.astype(float))
+        dy = np.diff(dataset.y.astype(float))
+        seg_lengths = np.sqrt(dx ** 2 + dy ** 2)
+        total_xy = float(seg_lengths.sum())
+        total_u  = dataset.u_max - dataset.u_min
+        if total_u > 0 and abs(total_xy - total_u) / total_u > 0.05:
+            warnings_.append(
+                f"Reference line arc length {total_xy:.3f} m differs from "
+                f"u range {total_u:.3f} m by more than 5%"
+            )
+
+    for w in warnings_:
+        warnings.warn(f"CRGDataset check: {w}", UserWarning, stacklevel=2)
+
+    if errors:
+        msg = "CRGDataset validation failed:\n" + "\n".join(f"  - {e}" for e in errors)
+        if raise_on_error:
+            raise ValueError(msg)
+        warnings.warn(msg, UserWarning, stacklevel=2)
+        return False
+
+    return True

crgutils/_contact_point.py

@@ -0,0 +1,167 @@
+"""ContactPoint: stateful evaluation context for a CRGDataset."""
+
+from __future__ import annotations
+
+from collections import deque
+from typing import Any
+
+import numpy as np
+
+from ._dataset import CRGDataset
+from ._types import EvalOptions
+from ._eval import (
+    _eval_uv_to_z,
+    _eval_uv_to_xy,
+    _xy_to_uv,
+    _eval_uv_to_pk,
+)
+
+
+class ContactPoint:
+    """Evaluation context for a :class:`CRGDataset`.
+
+    A ``ContactPoint`` binds a dataset to a set of :class:`EvalOptions` and
+    maintains a small history deque that accelerates successive nearby queries
+    (warm start for the xy→uv search).
+
+    Multiple ``ContactPoint`` instances on the same dataset are safe to use
+    independently (each has its own history and options).
+
+    Parameters
+    ----------
+    dataset:
+        The CRG dataset to evaluate against.
+    **options:
+        Keyword arguments that override :class:`EvalOptions` defaults.
+        Keys must be valid :class:`EvalOptions` field names.
+
+    Examples
+    --------
+    >>> import crgutils
+    >>> ds = crgutils.load("road.crg")
+    >>> cp = crgutils.ContactPoint(ds)
+    >>> z = cp.eval_uv_to_z(5.0, 0.0)
+    >>> x, y = cp.eval_uv_to_xy(5.0, 0.5)
+    >>> u, v = cp.eval_xy_to_uv(x, y)
+    """
+
+    def __init__(self, dataset: CRGDataset, **options: Any) -> None:
+        self._ds = dataset
+        # Merge dataset-level default options with caller overrides
+        merged = {**dataset.default_options, **options}
+        self._options = EvalOptions(**merged)
+        self._history: deque[tuple[float, float, int]] = deque(
+            maxlen=self._options.history_size
+        )
+
+        # Pre-seed history if ref_line_search_u is set
+        if self._options.ref_line_search_u is not None:
+            su = self._options.ref_line_search_u
+            idx = int((su - dataset.u_min) / dataset.u_inc)
+            idx = max(1, min(idx, dataset.n_u - 1))
+            x_seed, y_seed = _eval_uv_to_xy(dataset, self._options, su, 0.0)
+            self._history.appendleft((x_seed, y_seed, idx))
+
+    # ------------------------------------------------------------------
+    # Factory
+    # ------------------------------------------------------------------
+
+    def with_options(self, **overrides: Any) -> "ContactPoint":
+        """Return a new ``ContactPoint`` with option overrides applied.
+
+        The new instance shares the same dataset but gets a fresh history.
+        """
+        from dataclasses import asdict
+        current = asdict(self._options)
+        current.update(overrides)
+        return ContactPoint(self._ds, **current)
+
+    # ------------------------------------------------------------------
+    # Properties
+    # ------------------------------------------------------------------
+
+    @property
+    def options(self) -> EvalOptions:
+        return self._options
+
+    @property
+    def dataset(self) -> CRGDataset:
+        return self._ds
+
+    # ------------------------------------------------------------------
+    # Primary evaluation methods
+    # ------------------------------------------------------------------
+
+    def eval_uv_to_z(self, u: float, v: float) -> float:
+        """Bilinear elevation at road coordinate (u, v) [m]."""
+        return _eval_uv_to_z(self._ds, self._options, u, v)
+
+    def eval_xy_to_z(self, x: float, y: float) -> float:
+        """Elevation at Cartesian position (x, y) [m]."""
+        u, v = _xy_to_uv(self._ds, self._options, x, y, self._history)
+        return _eval_uv_to_z(self._ds, self._options, u, v)
+
+    def eval_uv_to_xy(self, u: float, v: float) -> tuple[float, float]:
+        """Convert road (u, v) to Cartesian (x, y) [m]."""
+        return _eval_uv_to_xy(self._ds, self._options, u, v)
+
+    def eval_xy_to_uv(self, x: float, y: float) -> tuple[float, float]:
+        """Convert Cartesian (x, y) to road (u, v) [m]."""
+        return _xy_to_uv(self._ds, self._options, x, y, self._history)
+
+    def eval_uv_to_pk(self, u: float, v: float) -> tuple[float, float]:
+        """Heading [rad] and curvature [1/m] at road (u, v)."""
+        return _eval_uv_to_pk(self._ds, self._options, u, v)
+
+    def eval_xy_to_pk(self, x: float, y: float) -> tuple[float, float]:
+        """Heading [rad] and curvature [1/m] at Cartesian (x, y)."""
+        u, v = _xy_to_uv(self._ds, self._options, x, y, self._history)
+        return _eval_uv_to_pk(self._ds, self._options, u, v)
+
+    # ------------------------------------------------------------------
+    # Vectorised convenience methods
+    # ------------------------------------------------------------------
+
+    def eval_uv_to_z_grid(
+        self,
+        u: "np.ndarray",
+        v: "np.ndarray",
+    ) -> "np.ndarray":
+        """Vectorised elevation evaluation over arrays of (u, v) pairs.
+
+        Parameters
+        ----------
+        u, v:
+            1-D arrays of matching length or broadcastable shapes.
+
+        Returns
+        -------
+        np.ndarray
+            Elevation values, same shape as broadcast(u, v).
+        """
+        u_arr = np.asarray(u, dtype=float).ravel()
+        v_arr = np.asarray(v, dtype=float).ravel()
+        z_arr = np.empty(len(u_arr), dtype=float)
+        for i in range(len(u_arr)):
+            z_arr[i] = _eval_uv_to_z(self._ds, self._options, float(u_arr[i]), float(v_arr[i]))
+        return z_arr
+
+    def eval_xy_to_z_grid(
+        self,
+        x: "np.ndarray",
+        y: "np.ndarray",
+    ) -> "np.ndarray":
+        """Vectorised elevation evaluation over arrays of (x, y) pairs."""
+        x_arr = np.asarray(x, dtype=float).ravel()
+        y_arr = np.asarray(y, dtype=float).ravel()
+        z_arr = np.empty(len(x_arr), dtype=float)
+        for i in range(len(x_arr)):
+            z_arr[i] = self.eval_xy_to_z(float(x_arr[i]), float(y_arr[i]))
+        return z_arr
+
+    def __repr__(self) -> str:
+        return (
+            f"ContactPoint(dataset={self._ds.source_file!r}, "
+            f"border_mode_u={self._options.border_mode_u.name}, "
+            f"border_mode_v={self._options.border_mode_v.name})"
+        )

crgutils/_convenience.py

@@ -0,0 +1,96 @@
+"""Convenience functions that combine multiple crgutils steps."""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+from ._dataset import CRGDataset
+from ._loader import load
+from ._check import check
+from ._modifiers import apply_modifiers
+from ._types import GridNaNMode
+
+
+def read(
+    path: str | Path,
+    *,
+    raise_on_error: bool = True,
+    # ---- scale modifiers ----
+    scale_z:               float | None = None,
+    scale_slope:           float | None = None,
+    scale_bank:            float | None = None,
+    scale_length:          float | None = None,
+    scale_width:           float | None = None,
+    scale_curvature:       float | None = None,
+    # ---- NaN handling ----
+    grid_nan_mode:         int | str | GridNaNMode | None = None,
+    grid_nan_offset:       float | None = None,
+    # ---- repositioning by reference point ----
+    ref_point_u:           float | None = None,
+    ref_point_u_frac:      float | None = None,
+    ref_point_u_offset:    float | None = None,
+    ref_point_v:           float | None = None,
+    ref_point_v_frac:      float | None = None,
+    ref_point_v_offset:    float | None = None,
+    ref_point_x:           float | None = None,
+    ref_point_y:           float | None = None,
+    ref_point_z:           float | None = None,
+    ref_point_phi:         float | None = None,
+    # ---- reference line offsets / rotation ----
+    ref_line_offset_x:     float | None = None,
+    ref_line_offset_y:     float | None = None,
+    ref_line_offset_z:     float | None = None,
+    ref_line_offset_phi:   float | None = None,
+    ref_line_rot_center_x: float | None = None,
+    ref_line_rot_center_y: float | None = None,
+) -> CRGDataset | None:
+    """Load a .crg file, validate it, and apply modifiers in one call.
+
+    Equivalent to::
+
+        ds = crgutils.load(path)
+        crgutils.check(ds, raise_on_error=True)
+        crgutils.apply_modifiers(ds, **ds.pending_modifiers)
+        ds.pending_modifiers.clear()
+
+    File-embedded ``pending_modifiers`` are always applied first. Any modifier
+    kwargs passed here override same-named keys from the file's embedded
+    modifiers.
+
+    Parameters
+    ----------
+    path:
+        Path to a ``.crg`` file.
+    raise_on_error:
+        If ``True`` (default), raise :exc:`ValueError` on validation failure.
+        If ``False``, return ``None`` instead.
+    scale_z, scale_slope, ... :
+        Modifier overrides — see :func:`~crgutils.apply_modifiers`.
+
+    Returns
+    -------
+    CRGDataset | None
+        The loaded, validated, modified dataset; or ``None`` if
+        *raise_on_error* is ``False`` and validation fails.
+    """
+    ds = load(path)
+    if not check(ds, raise_on_error=raise_on_error):
+        return None
+
+    # Build dict of explicitly-supplied caller overrides (skip None sentinels)
+    _locs = locals()
+    _modifier_keys = [
+        "scale_z", "scale_slope", "scale_bank", "scale_length",
+        "scale_width", "scale_curvature", "grid_nan_mode", "grid_nan_offset",
+        "ref_point_u", "ref_point_u_frac", "ref_point_u_offset",
+        "ref_point_v", "ref_point_v_frac", "ref_point_v_offset",
+        "ref_point_x", "ref_point_y", "ref_point_z", "ref_point_phi",
+        "ref_line_offset_x", "ref_line_offset_y", "ref_line_offset_z",
+        "ref_line_offset_phi", "ref_line_rot_center_x", "ref_line_rot_center_y",
+    ]
+    caller_overrides = {k: _locs[k] for k in _modifier_keys if _locs[k] is not None}
+
+    merged = {**ds.pending_modifiers, **caller_overrides}
+    apply_modifiers(ds, **merged)
+    ds.pending_modifiers.clear()
+    return ds

crgutils/_dataset.py

@@ -0,0 +1,192 @@
+"""CRGDataset: the core data container for a loaded .crg file."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import TYPE_CHECKING, Any
+
+if TYPE_CHECKING:
+    from ._contact_point import ContactPoint
+    from ._types import BorderMode, CurvMode, RefLineContinue
+
+import numpy as np
+
+
+@dataclass
+class CRGDataset:
+    """Holds all arrays and metadata parsed from a single .crg file.
+
+    Coordinate system
+    -----------------
+    u  — arc-length along the reference line [m]
+    v  — lateral offset from the reference line [m]
+    x, y — Cartesian world coordinates of the reference line [m]
+    phi  — heading angle of the reference line [rad]
+    z  — elevation grid, shape (n_v, n_u) [m], float32
+
+    All arrays are stored in increasing u / v order.
+    """
+
+    # --- Reference line (all float64, shape (n_u,)) ---
+    u:   np.ndarray   # u coordinates
+    x:   np.ndarray   # Cartesian x of ref line (derived from phi if absent)
+    y:   np.ndarray   # Cartesian y of ref line
+    phi: np.ndarray   # heading angles [rad]
+
+    # --- Cross-section grid ---
+    v: np.ndarray     # v positions, shape (n_v,), float64
+    z: np.ndarray     # elevation grid, shape (n_v, n_u), float32
+    z_mean: np.ndarray  # per-v-channel mean subtracted during normalisation, (n_v,)
+
+    # --- Optional reference line channels (shape (n_u,) or scalar) ---
+    ref_z: np.ndarray | None   # ref line elevation [m]
+    bank:  np.ndarray | None   # banking [1/m]  (tan of bank angle)
+    slope: np.ndarray | None   # slope [1]
+
+    # --- Grid spacing / topology ---
+    u_inc:             float
+    v_inc:             float = 0.0   # 0 if v is non-equally spaced
+    u_is_closed:       bool  = False
+    u_close_min:       float = 0.0
+    u_close_max:       float = 0.0
+    v_equally_spaced:  bool  = True
+    has_bank:          bool  = False
+
+    # --- Precomputed trigonometry for extrapolation ---
+    phi_first_sin: float = 0.0
+    phi_first_cos: float = 1.0
+    phi_last_sin:  float = 0.0
+    phi_last_cos:  float = 1.0
+
+    # --- Options / modifiers embedded in the file header ---
+    default_options:   dict[str, Any] = field(default_factory=dict)
+    pending_modifiers: dict[str, Any] = field(default_factory=dict)
+
+    # --- Source info (informational only) ---
+    source_file: str = ""
+    comment:     str = ""
+
+    # ------------------------------------------------------------------
+    # Properties
+    # ------------------------------------------------------------------
+
+    @property
+    def u_min(self) -> float:
+        return float(self.u[0])
+
+    @property
+    def u_max(self) -> float:
+        return float(self.u[-1])
+
+    @property
+    def v_min(self) -> float:
+        return float(self.v[0])
+
+    @property
+    def v_max(self) -> float:
+        return float(self.v[-1])
+
+    @property
+    def u_range(self) -> tuple[float, float]:
+        return (self.u_min, self.u_max)
+
+    @property
+    def v_range(self) -> tuple[float, float]:
+        return (self.v_min, self.v_max)
+
+    @property
+    def shape(self) -> tuple[int, int]:
+        """(n_v, n_u)"""
+        return self.z.shape
+
+    @property
+    def n_u(self) -> int:
+        return len(self.u)
+
+    @property
+    def n_v(self) -> int:
+        return len(self.v)
+
+    # ------------------------------------------------------------------
+    # Human-readable summaries
+    # ------------------------------------------------------------------
+
+    def info(self) -> str:
+        lines = [
+            f"CRGDataset: {self.source_file}",
+            f"  u range : [{self.u_min:.4f}, {self.u_max:.4f}] m  (n={self.n_u}, inc={self.u_inc:.4f})",
+            f"  v range : [{self.v_min:.4f}, {self.v_max:.4f}] m  (n={self.n_v}"
+            + (f", inc={self.v_inc:.4f})" if self.v_equally_spaced else ", irregular)"),
+            f"  z shape : {self.z.shape}  (float32)",
+            f"  has_bank: {self.has_bank}",
+            f"  closed  : {self.u_is_closed}",
+        ]
+        return "\n".join(lines)
+
+    def channel_info(self) -> str:
+        n_channels = 1 + self.n_v  # U virtual + v channels
+        if self.phi is not None and len(self.phi):
+            n_channels += 1
+        lines = [f"Channel count: {n_channels}"]
+        lines.append(f"  U  : u coordinate, virtual, [{self.u_min:.4f}, {self.u_max:.4f}] m")
+        if self.phi is not None and len(self.phi):
+            lines.append(f"  phi: heading angle, [{float(self.phi.min()):.4f}, {float(self.phi.max()):.4f}] rad")
+        for i, vpos in enumerate(self.v):
+            zch = self.z[i]
+            lines.append(
+                f"  D{i+1} : v={vpos:+.4f} m, z in [{float(np.nanmin(zch)+self.z_mean[i]):.4f}, "
+                f"{float(np.nanmax(zch)+self.z_mean[i]):.4f}] m"
+            )
+        return "\n".join(lines)
+
+    def road_info(self) -> str:
+        lines = [
+            "Road information:",
+            f"  reference line : u=[{self.u_min:.4f}, {self.u_max:.4f}] m",
+            f"  x range        : [{float(self.x.min()):.4f}, {float(self.x.max()):.4f}] m",
+            f"  y range        : [{float(self.y.min()):.4f}, {float(self.y.max()):.4f}] m",
+            f"  phi range      : [{float(self.phi.min()):.4f}, {float(self.phi.max()):.4f}] rad",
+            f"  v range        : [{self.v_min:.4f}, {self.v_max:.4f}] m",
+        ]
+        z_all = self.z.astype(np.float64) + self.z_mean[:, np.newaxis]
+        lines.append(
+            f"  z range        : [{float(np.nanmin(z_all)):.4f}, {float(np.nanmax(z_all)):.4f}] m"
+        )
+        return "\n".join(lines)
+
+    def create_contact_point(
+        self,
+        *,
+        border_mode_u:     BorderMode | None      = None,
+        border_mode_v:     BorderMode | None      = None,
+        border_offset_u:   float | None           = None,
+        border_offset_v:   float | None           = None,
+        smooth_u_begin:    float | None           = None,
+        smooth_u_end:      float | None           = None,
+        ref_line_continue: RefLineContinue | None = None,
+        curv_mode:         CurvMode | None        = None,
+        ref_line_search_u: float | None           = None,
+        ref_line_close:    float | None           = None,
+        ref_line_far:      float | None           = None,
+        history_size:      int | None             = None,
+    ) -> ContactPoint:
+        """Create a :class:`ContactPoint` evaluation context for this dataset.
+
+        All parameters correspond to fields of :class:`~crgutils.EvalOptions`.
+        Omitted parameters use the dataset's ``default_options`` or the
+        :class:`~crgutils.EvalOptions` defaults.
+
+        Returns
+        -------
+        ContactPoint
+            A new evaluation context bound to this dataset.
+        """
+        from ._contact_point import ContactPoint  # lazy import — avoids circular dep
+        _locs = locals()
+        _option_keys = [
+            "border_mode_u", "border_mode_v", "border_offset_u", "border_offset_v",
+            "smooth_u_begin", "smooth_u_end", "ref_line_continue", "curv_mode",
+            "ref_line_search_u", "ref_line_close", "ref_line_far", "history_size",
+        ]
+        options = {k: _locs[k] for k in _option_keys if _locs[k] is not None}
+        return ContactPoint(self, **options)