planarcurve 0.1.0__py3-none-any.whl

planarcurve/__init__.py

@@ -0,0 +1,52 @@
+"""Planar curve and trajectory mathematics."""
+
+from .trajectory import (
+    HeadingRateChange,
+    HeadingRateProfile,
+    Pose2D,
+    Trajectory2D,
+    TrajectorySample,
+    curvature,
+    evaluate_heading,
+    evaluate_heading_rate,
+    evaluate_trajectory,
+    evaluate_trajectory_at,
+    integrated_heading_rate,
+    sample_trajectory_by_arclength,
+    trajectory_end_pose,
+    trajectory_to_cubic_path,
+    trajectory_total_turn,
+)
+from .transforms import Affine2D, Similarity2D
+from .types import CubicBezier, CubicPath, CubicSegment, Point2D, QuadraticBezier
+
+try:
+    from ._version import __version__
+except ImportError:  # source tree before setuptools_scm generated _version.py
+    __version__ = "0+unknown"
+
+__all__ = [
+    "Affine2D",
+    "CubicBezier",
+    "CubicPath",
+    "QuadraticBezier",
+    "CubicSegment",
+    "HeadingRateChange",
+    "HeadingRateProfile",
+    "Point2D",
+    "Pose2D",
+    "Similarity2D",
+    "Trajectory2D",
+    "TrajectorySample",
+    "__version__",
+    "curvature",
+    "evaluate_heading",
+    "evaluate_heading_rate",
+    "evaluate_trajectory",
+    "evaluate_trajectory_at",
+    "integrated_heading_rate",
+    "sample_trajectory_by_arclength",
+    "trajectory_end_pose",
+    "trajectory_to_cubic_path",
+    "trajectory_total_turn",
+]

planarcurve/_version.py

@@ -0,0 +1,24 @@
+# file generated by vcs-versioning
+# don't change, don't track in version control
+from __future__ import annotations
+
+__all__ = [
+    "__version__",
+    "__version_tuple__",
+    "version",
+    "version_tuple",
+    "__commit_id__",
+    "commit_id",
+]
+
+version: str
+__version__: str
+__version_tuple__: tuple[int | str, ...]
+version_tuple: tuple[int | str, ...]
+commit_id: str | None
+__commit_id__: str | None
+
+__version__ = version = '0.1.0'
+__version_tuple__ = version_tuple = (0, 1, 0)
+
+__commit_id__ = commit_id = 'g96d8146d2'

planarcurve/bezier.py

@@ -0,0 +1,211 @@
+from __future__ import annotations
+
+import numpy as np
+
+from .types import CubicBezier, QuadraticBezier
+
+
+def _parameters(t: np.ndarray) -> np.ndarray:
+    values = np.asarray(t, dtype=float)
+    if values.ndim != 1 or not np.isfinite(values).all():
+        raise ValueError("parameters must be a finite one-dimensional array")
+    if np.any((values < 0.0) | (values > 1.0)):
+        raise ValueError("parameters must lie in [0, 1]")
+    return values
+
+
+def evaluate_quadratic(p0, p1, p2, t: np.ndarray) -> np.ndarray:
+    u = _parameters(t)[:, None]
+    one = 1.0 - u
+    return one**2 * np.asarray(p0) + 2.0 * one * u * np.asarray(p1) + u**2 * np.asarray(p2)
+
+
+def evaluate_quadratic_bezier(curve: QuadraticBezier, t: np.ndarray) -> np.ndarray:
+    return evaluate_quadratic(curve.p0, curve.p1, curve.p2, t)
+
+
+def quadratic_derivative(curve: QuadraticBezier, t: np.ndarray) -> np.ndarray:
+    u = _parameters(t)[:, None]
+    return 2.0 * (
+        (1.0 - u) * (np.asarray(curve.p1) - np.asarray(curve.p0))
+        + u * (np.asarray(curve.p2) - np.asarray(curve.p1))
+    )
+
+
+def evaluate_cubic(curve: CubicBezier, t: np.ndarray) -> np.ndarray:
+    u = _parameters(t)[:, None]
+    one = 1.0 - u
+    return (
+        one**3 * np.asarray(curve.p0)
+        + 3.0 * one**2 * u * np.asarray(curve.p1)
+        + 3.0 * one * u**2 * np.asarray(curve.p2)
+        + u**3 * np.asarray(curve.p3)
+    )
+
+
+def cubic_derivative(curve: CubicBezier, t: np.ndarray) -> np.ndarray:
+    u = _parameters(t)[:, None]
+    one = 1.0 - u
+    return 3.0 * (
+        one**2 * (np.asarray(curve.p1) - np.asarray(curve.p0))
+        + 2.0 * one * u * (np.asarray(curve.p2) - np.asarray(curve.p1))
+        + u**2 * (np.asarray(curve.p3) - np.asarray(curve.p2))
+    )
+
+
+def cubic_second_derivative(curve: CubicBezier, t: np.ndarray) -> np.ndarray:
+    u = _parameters(t)[:, None]
+    return 6.0 * (
+        (1.0 - u) * (np.asarray(curve.p2) - 2.0 * np.asarray(curve.p1) + np.asarray(curve.p0))
+        + u * (np.asarray(curve.p3) - 2.0 * np.asarray(curve.p2) + np.asarray(curve.p1))
+    )
+
+
+def cubic_tangent(curve: CubicBezier, t: np.ndarray, *, unit: bool = True) -> np.ndarray:
+    derivative = cubic_derivative(curve, t)
+    if not unit:
+        return derivative
+    norms = np.linalg.norm(derivative, axis=1)
+    return np.divide(
+        derivative,
+        norms[:, None],
+        out=np.zeros_like(derivative),
+        where=norms[:, None] > 1e-12,
+    )
+
+
+def _signed_curvature(first: np.ndarray, second: np.ndarray) -> np.ndarray:
+    cross = first[:, 0] * second[:, 1] - first[:, 1] * second[:, 0]
+    speed = np.linalg.norm(first, axis=1)
+    return np.divide(cross, speed**3, out=np.zeros_like(cross), where=speed > 1e-12)
+
+
+def cubic_curvature(curve: CubicBezier, t: np.ndarray) -> np.ndarray:
+    return _signed_curvature(cubic_derivative(curve, t), cubic_second_derivative(curve, t))
+
+
+def quadratic_tangent(
+    curve: QuadraticBezier, t: np.ndarray, *, unit: bool = True
+) -> np.ndarray:
+    derivative = quadratic_derivative(curve, t)
+    if not unit:
+        return derivative
+    norms = np.linalg.norm(derivative, axis=1)
+    return np.divide(
+        derivative,
+        norms[:, None],
+        out=np.zeros_like(derivative),
+        where=norms[:, None] > 1e-12,
+    )
+
+
+def quadratic_curvature(curve: QuadraticBezier, t: np.ndarray) -> np.ndarray:
+    derivative = quadratic_derivative(curve, t)
+    second = np.broadcast_to(
+        2.0 * (np.asarray(curve.p2) - 2.0 * np.asarray(curve.p1) + np.asarray(curve.p0)),
+        derivative.shape,
+    )
+    return _signed_curvature(derivative, second)
+
+
+def split_cubic(curve: CubicBezier, t: float = 0.5) -> tuple[CubicBezier, CubicBezier]:
+    parameter = _parameters(np.array([t]))[0]
+    p0, p1, p2, p3 = (
+        np.asarray(point, dtype=float)
+        for point in (curve.p0, curve.p1, curve.p2, curve.p3)
+    )
+    p01 = (1 - parameter) * p0 + parameter * p1
+    p12 = (1 - parameter) * p1 + parameter * p2
+    p23 = (1 - parameter) * p2 + parameter * p3
+    p012 = (1 - parameter) * p01 + parameter * p12
+    p123 = (1 - parameter) * p12 + parameter * p23
+    middle = (1 - parameter) * p012 + parameter * p123
+    return (
+        CubicBezier(tuple(p0), tuple(p01), tuple(p012), tuple(middle)),
+        CubicBezier(tuple(middle), tuple(p123), tuple(p23), tuple(p3)),
+    )
+
+
+def _point_line_distance(point: np.ndarray, start: np.ndarray, end: np.ndarray) -> float:
+    direction = end - start
+    norm = np.linalg.norm(direction)
+    if norm <= 1e-12:
+        return float(np.linalg.norm(point - start))
+    offset = point - start
+    cross = direction[0] * offset[1] - direction[1] * offset[0]
+    return float(abs(cross) / norm)
+
+
+def flatten_cubic(
+    curve: CubicBezier, *, tolerance: float = 1e-3, max_depth: int = 16
+) -> np.ndarray:
+    if not np.isfinite(tolerance) or tolerance <= 0.0:
+        raise ValueError("tolerance must be positive and finite")
+    if isinstance(max_depth, bool) or max_depth < 0:
+        raise ValueError("max_depth must be nonnegative")
+    points = [np.asarray(curve.p0, dtype=float)]
+
+    def flatten(piece: CubicBezier, depth: int) -> None:
+        p0, p1, p2, p3 = (
+            np.asarray(point, dtype=float) for point in (piece.p0, piece.p1, piece.p2, piece.p3)
+        )
+        flatness = max(_point_line_distance(p1, p0, p3), _point_line_distance(p2, p0, p3))
+        if flatness <= tolerance:
+            points.append(p3)
+            return
+        if depth == 0:
+            raise ValueError("cubic flattening did not meet tolerance")
+        left, right = split_cubic(piece)
+        flatten(left, depth - 1)
+        flatten(right, depth - 1)
+
+    flatten(curve, max_depth)
+    return np.asarray(points)
+
+
+def cubic_length(curve: CubicBezier, *, tolerance: float = 1e-6, max_depth: int = 16) -> float:
+    if not np.isfinite(tolerance) or tolerance <= 0.0:
+        raise ValueError("tolerance must be positive and finite")
+    if isinstance(max_depth, bool) or max_depth < 0:
+        raise ValueError("max_depth must be nonnegative")
+
+    def estimate(piece: CubicBezier, local_tolerance: float, depth: int) -> float:
+        p0, p1, p2, p3 = (np.asarray(point) for point in (piece.p0, piece.p1, piece.p2, piece.p3))
+        chord = float(np.linalg.norm(p3 - p0))
+        polygon = float(
+            np.linalg.norm(p1 - p0) + np.linalg.norm(p2 - p1) + np.linalg.norm(p3 - p2)
+        )
+        if polygon - chord <= local_tolerance:
+            return (polygon + chord) / 2.0
+        if depth == 0:
+            raise ValueError("cubic length approximation did not meet tolerance")
+        left, right = split_cubic(piece)
+        return estimate(left, local_tolerance / 2.0, depth - 1) + estimate(
+            right, local_tolerance / 2.0, depth - 1
+        )
+
+    return estimate(curve, tolerance, max_depth)
+
+
+def _quadratic_as_cubic(curve: QuadraticBezier) -> CubicBezier:
+    p0, p1, p2 = (np.asarray(point, dtype=float) for point in (curve.p0, curve.p1, curve.p2))
+    return CubicBezier(
+        tuple(p0),
+        tuple(p0 + (2.0 / 3.0) * (p1 - p0)),
+        tuple(p2 + (2.0 / 3.0) * (p1 - p2)),
+        tuple(p2),
+    )
+
+
+def flatten_quadratic(
+    curve: QuadraticBezier, *, tolerance: float = 1e-3, max_depth: int = 16
+) -> np.ndarray:
+    return flatten_cubic(
+        _quadratic_as_cubic(curve), tolerance=tolerance, max_depth=max_depth
+    )
+
+
+def quadratic_length(
+    curve: QuadraticBezier, *, tolerance: float = 1e-6, max_depth: int = 16
+) -> float:
+    return cubic_length(_quadratic_as_cubic(curve), tolerance=tolerance, max_depth=max_depth)

planarcurve/curvature.py

@@ -0,0 +1,82 @@
+"""Compatibility imports for the former curvature-module API.
+
+New code should import the neutral trajectory API from :mod:`planarcurve.trajectory`.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+from .trajectory import (
+    HeadingRateChange,
+    HeadingRateProfile,
+    Pose2D,
+    Trajectory2D,
+    TrajectorySample,
+    evaluate_heading,
+    evaluate_heading_rate,
+    evaluate_trajectory,
+    evaluate_trajectory_at,
+    integrated_heading_rate,
+    sample_trajectory_by_arclength,
+    trajectory_end_pose,
+    trajectory_to_cubic_path,
+    trajectory_total_turn,
+)
+
+__all__ = [
+    "CurvatureChange",
+    "CurvatureProfile",
+    "CurvatureSample",
+    "CurvatureStrokeGeometry",
+    "CurvatureStrokeSample",
+    "CurvatureTrajectory",
+    "Pose2D",
+    "StrokePose",
+    "evaluate_curvature_stroke",
+    "evaluate_heading",
+    "evaluate_heading_rate",
+    "evaluate_stroke_at",
+    "integrated_heading_rate",
+    "sample_by_arclength",
+    "sample_curvature_stroke",
+    "to_cubic_beziers",
+    "trajectory_end_pose",
+    "trajectory_total_turn",
+]
+
+
+CurvatureChange = HeadingRateChange
+CurvatureProfile = HeadingRateProfile
+CurvatureTrajectory = Trajectory2D
+CurvatureStrokeGeometry = Trajectory2D
+CurvatureSample = TrajectorySample
+CurvatureStrokeSample = TrajectorySample
+StrokePose = Pose2D
+evaluate_curvature_stroke = evaluate_trajectory
+evaluate_stroke_at = evaluate_trajectory_at
+
+
+def curvature_to_cubic_segments(
+    trajectory: Trajectory2D, *, max_error: float = 1e-4
+):
+    return list(trajectory_to_cubic_path(trajectory, max_error=max_error).segments)
+
+
+def sample_curvature_stroke(
+    trajectory: Trajectory2D, *, count: int = 256, samples: int = 64
+) -> np.ndarray:
+    return sample_trajectory_by_arclength(
+        trajectory, samples, integration_count=count
+    )
+
+
+def sample_by_arclength(
+    trajectory: Trajectory2D, count: int, *, integration_count: int = 256
+) -> np.ndarray:
+    return sample_trajectory_by_arclength(
+        trajectory, count, integration_count=integration_count
+    )
+
+
+to_cubic_beziers = trajectory_to_cubic_path

planarcurve/fitting/__init__.py

@@ -0,0 +1,3 @@
+from .policy import SplineEndpointPolicy
+
+__all__ = ["SplineEndpointPolicy"]

planarcurve/fitting/parameters.py

@@ -0,0 +1,110 @@
+"""Generic parameterizations for ordered positions, intervals, and positives."""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+def ordered_positions_from_logits(
+    logits: np.ndarray,
+    *,
+    min_spacing: float = 0.0,
+    start: float = 0.0,
+    end: float = 1.0,
+) -> np.ndarray:
+    """Decode interval logits into strictly ordered positions in ``(start, end)``."""
+    values = np.asarray(logits, dtype=float)
+    if values.ndim != 1 or not np.isfinite(values).all():
+        raise ValueError("logits must be a finite 1-D array")
+    _validate_interval(start, end)
+    _validate_spacing(min_spacing)
+    interval_count = values.size
+    position_count = max(interval_count - 1, 0)
+    if position_count == 0:
+        return np.empty(0, dtype=float)
+    span = end - start
+    reserved = max(position_count - 1, 0) * min_spacing
+    if reserved >= span:
+        raise ValueError("interval too small for the requested positions and spacing")
+    weights = np.exp(np.maximum(values - values.max(), -27.0))
+    fractions = weights / weights.sum()
+    gaps = fractions * (span - reserved)
+    gaps[1:-1] += min_spacing
+    return start + np.cumsum(gaps[:-1])
+
+
+def ordered_positions_to_logits(
+    positions: np.ndarray,
+    *,
+    min_spacing: float = 0.0,
+    start: float = 0.0,
+    end: float = 1.0,
+) -> np.ndarray:
+    """Encode ordered positions as interval logits, up to a constant offset."""
+    values = np.asarray(positions, dtype=float)
+    if values.ndim != 1 or not np.isfinite(values).all():
+        raise ValueError("positions must be a finite 1-D array")
+    _validate_interval(start, end)
+    _validate_spacing(min_spacing)
+    if values.size == 0:
+        return np.empty(0, dtype=float)
+    if np.any(np.diff(values) <= 0.0) or values[0] <= start or values[-1] >= end:
+        raise ValueError("positions must be strictly ordered inside the interval")
+    gaps = np.diff(np.concatenate(([start], values, [end])))
+    if np.any(gaps[1:-1] < min_spacing - 1e-12):
+        raise ValueError(f"positions must respect minimum spacing {min_spacing}")
+    available = (end - start) - max(values.size - 1, 0) * min_spacing
+    fractions = gaps.copy()
+    fractions[1:-1] -= min_spacing
+    fractions = np.clip(fractions / available, 1e-12, None)
+    return np.log(fractions)
+
+
+def decode_interval(param: float, *, low: float, high: float) -> float:
+    """Map an unconstrained scalar into the open interval ``(low, high)``."""
+    _validate_interval(low, high)
+    value = float(param)
+    if not np.isfinite(value):
+        raise ValueError("parameter must be finite")
+    if value >= 0.0:
+        logistic = 1.0 / (1.0 + np.exp(-value))
+    else:
+        exponential = np.exp(value)
+        logistic = exponential / (1.0 + exponential)
+    return float(low + (high - low) * logistic)
+
+
+def encode_interval(value: float, *, low: float, high: float) -> float:
+    """Map an interval value back to its unconstrained logistic parameter."""
+    _validate_interval(low, high)
+    numeric = float(value)
+    if not np.isfinite(numeric) or not low <= numeric <= high:
+        raise ValueError("value must be finite and inside the interval")
+    fraction = np.clip((numeric - low) / (high - low), 1e-12, 1.0 - 1e-12)
+    return float(np.log(fraction / (1.0 - fraction)))
+
+
+def decode_positive(param: float) -> float:
+    """Decode a real value as a positive scalar."""
+    value = float(param)
+    if not np.isfinite(value):
+        raise ValueError("parameter must be finite")
+    return float(np.exp(np.clip(value, -50.0, 50.0)))
+
+
+def encode_positive(value: float) -> float:
+    """Encode a positive scalar as its logarithm."""
+    numeric = float(value)
+    if not np.isfinite(numeric) or numeric <= 0.0:
+        raise ValueError("value must be positive and finite")
+    return float(np.log(numeric))
+
+
+def _validate_interval(low: float, high: float) -> None:
+    if not np.isfinite([low, high]).all() or not low < high:
+        raise ValueError("interval bounds must be finite and increasing")
+
+
+def _validate_spacing(min_spacing: float) -> None:
+    if not np.isfinite(min_spacing) or min_spacing < 0.0:
+        raise ValueError("min_spacing must be finite and nonnegative")

planarcurve/fitting/policy.py

@@ -0,0 +1,17 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Literal
+
+
+@dataclass(frozen=True)
+class SplineEndpointPolicy:
+    """Endpoint constraints used by spline fitting, not by spline geometry."""
+
+    start: Literal["locked", "free", "anchored"] = "locked"
+    end: Literal["locked", "free", "anchored"] = "locked"
+
+    def __post_init__(self) -> None:
+        allowed = {"locked", "free", "anchored"}
+        if self.start not in allowed or self.end not in allowed:
+            raise ValueError("endpoint policy must be locked, free, or anchored")