feat(safety): native real-time safe-projection via PyO3

.gitignore

@@ -14,3 +14,6 @@ dist/
 .coverage
 htmlcov/
 .DS_Store
+
+# native PyO3 build artifact (build with maturin)
+ghostloop/_rt_safety*.so

ghostloop/policies/safe_projection.py

@@ -16,6 +16,15 @@
       Intent with corrected args + a ``"projected_from": orig_target``
       field for auditability.
 
+      When the optional native extension ``ghostloop._rt_safety`` is
+      built AND the workspace carries only Sphere obstacles, the
+      analytic geometry runs in the allocation-free Rust core instead
+      of pure Python. The result is numerically identical (verified to
+      1e-9 per axis in tests/test_rt_safety_equivalence.py); the
+      audit/return shape is unchanged. Any workspace with a box
+      obstacle, or any environment where the native module is not
+      built, transparently uses the pure-Python path below.
+
   ``project_to_sdf(intent, workspace, extras=())``
       Numerical: gradient-descent on the SDF until distance >= 0.
       Slower than the analytic version; handles convex polytopes /
@@ -39,6 +48,15 @@
 )
 
 
+# Optional allocation-free Rust fast path for the sphere-only analytic
+# projection. Guarded so ghostloop runs unchanged when the native extension
+# has not been built (the pure-Python path below is always available).
+try:
+    from .. import _rt_safety as _RT_SAFETY  # type: ignore[attr-defined]
+except Exception:  # noqa: BLE001 - any import/ABI failure must fall back cleanly.
+    _RT_SAFETY = None
+
+
 Point3 = tuple[float, float, float]
 
 
@@ -61,6 +79,52 @@ def _set_target(args: dict, p: Point3) -> dict:
     return args
 
 
+def _project_point_native(
+    target: Point3, workspace: WorkspaceModel,
+) -> Point3 | None:
+    """Run the sphere-only analytic projection in the native Rust core.
+
+    Returns the projected point, or ``None`` if the native path does not
+    apply (module not built, or the workspace has a non-sphere obstacle).
+    The numerics match the pure-Python branch below bit-for-bit: the Rust
+    core uses f64 and the identical operation order (clamp to bounds, then
+    sequential radial push-out per sphere, degenerate ``dist < 1e-9`` sets
+    ``p.x = center.x + forbidden_r`` and continues).
+    """
+    if _RT_SAFETY is None:
+        return None
+    spheres = []
+    for ob in workspace.obstacles:
+        if not isinstance(ob, Sphere):
+            # Box (or any future non-sphere) obstacle: the native core does
+            # not handle these analytically; defer to pure Python.
+            return None
+        spheres.append(
+            (
+                float(ob.center[0]),
+                float(ob.center[1]),
+                float(ob.center[2]),
+                float(ob.radius),
+                float(ob.inflation),
+            )
+        )
+    bmin = (
+        float(workspace.bounds_min[0]),
+        float(workspace.bounds_min[1]),
+        float(workspace.bounds_min[2]),
+    )
+    bmax = (
+        float(workspace.bounds_max[0]),
+        float(workspace.bounds_max[1]),
+        float(workspace.bounds_max[2]),
+    )
+    try:
+        out = _RT_SAFETY.project_to_workspace(target, bmin, bmax, spheres)
+    except Exception:  # noqa: BLE001 - never let the fast path break a repair.
+        return None
+    return float(out[0]), float(out[1]), float(out[2])
+
+
 def project_to_workspace(
     intent: Intent, workspace: WorkspaceModel,
 ) -> Intent:
@@ -73,32 +137,43 @@ def project_to_workspace(
     AxisAlignedBox obstacles are NOT pushed out of analytically (the
     closest-point-on-box-exterior is geometry that's annoying to
     compute robustly without numpy). For boxes use ``project_to_sdf``.
+
+    When ``ghostloop._rt_safety`` is built and the workspace has only
+    Sphere obstacles, the analytic geometry runs in the native Rust core;
+    the result and the ``projected_from`` audit field are identical to the
+    pure-Python computation. Otherwise the pure-Python path runs unchanged.
     """
     target = _extract_target(intent.args)
     if target is None:
         return intent
-    p = list(target)
-    # Clamp to outer bounds.
-    for i in range(3):
-        p[i] = max(workspace.bounds_min[i], min(workspace.bounds_max[i], p[i]))
-    # Radial pushout from each sphere.
-    for ob in workspace.obstacles:
-        if not isinstance(ob, Sphere):
-            continue
-        cx, cy, cz = ob.center
-        dx, dy, dz = p[0] - cx, p[1] - cy, p[2] - cz
-        dist = math.sqrt(dx * dx + dy * dy + dz * dz)
-        forbidden_r = ob.radius + ob.inflation
-        if dist < forbidden_r:
-            if dist < 1e-9:
-                # Degenerate: pick an arbitrary direction.
-                p[0] = cx + forbidden_r
+
+    native = _project_point_native(target, workspace)
+    if native is not None:
+        out_target = native
+    else:
+        p = list(target)
+        # Clamp to outer bounds.
+        for i in range(3):
+            p[i] = max(workspace.bounds_min[i], min(workspace.bounds_max[i], p[i]))
+        # Radial pushout from each sphere.
+        for ob in workspace.obstacles:
+            if not isinstance(ob, Sphere):
                 continue
-            scale = forbidden_r / dist
-            p[0] = cx + dx * scale
-            p[1] = cy + dy * scale
-            p[2] = cz + dz * scale
-    out_target = (p[0], p[1], p[2])
+            cx, cy, cz = ob.center
+            dx, dy, dz = p[0] - cx, p[1] - cy, p[2] - cz
+            dist = math.sqrt(dx * dx + dy * dy + dz * dz)
+            forbidden_r = ob.radius + ob.inflation
+            if dist < forbidden_r:
+                if dist < 1e-9:
+                    # Degenerate: pick an arbitrary direction.
+                    p[0] = cx + forbidden_r
+                    continue
+                scale = forbidden_r / dist
+                p[0] = cx + dx * scale
+                p[1] = cy + dy * scale
+                p[2] = cz + dz * scale
+        out_target = (p[0], p[1], p[2])
+
     if out_target == target:
         return intent
     new_args = _set_target(intent.args, out_target)

native/rt_safety/.gitignore

@@ -0,0 +1,2 @@
+/target
+/.m4venv

native/rt_safety/Cargo.toml

@@ -0,0 +1,23 @@
+[package]
+name = "rt_safety"
+version = "0.1.0"
+edition = "2021"
+authors = ["Joe Munene <joemunene984@gmail.com>"]
+description = "Allocation-free, real-time-grade port of ghostloop's safe-projection envelope, exposed to Python via PyO3."
+license = "MIT"
+
+[lib]
+# cdylib: the Python extension module (.so / .pyd).
+# rlib:   so the same logic can be unit-tested with `cargo test` and reused
+#         by other Rust crates (the M4 rt_control campaign core).
+name = "rt_safety"
+crate-type = ["cdylib", "rlib"]
+
+[dependencies]
+pyo3 = { version = "0.22", features = ["extension-module"] }
+
+[profile.release]
+opt-level = 3
+lto = true
+codegen-units = 1
+panic = "abort"

native/rt_safety/pyproject.toml

@@ -0,0 +1,21 @@
+# maturin build config for the rt_safety extension.
+#
+# This pyproject exists only so `maturin` knows how to compile the PyO3
+# cdylib (it injects the macOS `-undefined dynamic_lookup` link flag that a
+# bare `cargo build` of an extension-module cdylib lacks). The compiled
+# artifact is installed into the ghostloop package as `ghostloop._rt_safety`;
+# the parent ghostloop project itself stays a plain setuptools build.
+[build-system]
+requires = ["maturin>=1.7,<2"]
+build-backend = "maturin"
+
+[project]
+name = "ghostloop-rt-safety"
+version = "0.1.0"
+description = "Allocation-free real-time port of ghostloop's safe-projection envelope."
+requires-python = ">=3.10"
+
+[tool.maturin]
+# Install the compiled module at ghostloop._rt_safety so the in-tree shim's
+# `from ghostloop import _rt_safety` resolves.
+module-name = "ghostloop._rt_safety"