orisys-sensor-design/blender/scripts/probe_falloff.py

"""bpy-FREE probe geometry maths: proportional falloff, basis, edge stats.

Why the falloff is computed here instead of with an operator
------------------------------------------------------------
``bpy.ops.transform.translate(use_proportional_edit=True)`` is deliberately NOT
used anywhere in this pipeline. That operator is viewport-modal: it needs a 3D
view region to define the pivot and, crucially, its
``tool_settings.proportional_size`` is interpreted in SCREEN SPACE (pixels)
relative to that region. Under ``blender -b`` there is no 3D-view region and no
screen-space scale, so the same script would affect a different vertex set
depending on GUI state or zoom -- and could not be reproduced at all headlessly.
The falloff is therefore computed explicitly, in millimetres, by this module.

Semantics replicate Blender's proportional editing with the probe members as the
selection:

* ``d`` = distance from the **original** position of a vertex to the nearest
  probe vertex;
* ``t = d / radius_mm``;
* ``w = curve(t)``, with ``w = 0`` whenever ``t >= 1`` and ``w = 1`` for probe
  members.

Because ``d`` is measured from the original (base) coordinates, the weight
vector is constant across every frame of the sweep -- exactly Blender's
behaviour, where the proportional falloff is computed once from the selected
vertices' starting positions.

Only numpy is required, so the normal pytest suite imports this module without
bpy (the same pattern as ``blender/scripts/patch_geometry.py``).
"""

from __future__ import annotations

from typing import Any, Sequence

import numpy as np

#: Blender proportional-edit falloff modes accepted by the pipeline contract.
FALLOFF_MODES = ("smooth", "linear", "sharp", "constant")
DEFAULT_FALLOFF = "smooth"

#: Contract failure threshold for the probe-average basis ("not orthonormal
#: within 1e-6" must fail loudly). The measured Xynova basis is orthonormal to
#: ~2.2e-16.
ORTHONORMAL_TOLERANCE = 1e-6

#: Weight histogram bands, lowest-inclusive/highest-exclusive except "1.0".
#: Empty bands are omitted so the report only prints what actually moves.
_WEIGHT_BANDS = (
    ("1.0", 1.0, 1.0),
    ("0.5-1.0", 0.5, 1.0),
    ("0.3-0.5", 0.3, 0.5),
    ("0.1-0.3", 0.1, 0.3),
    ("0.05-0.1", 0.05, 0.1),
    ("0.0-0.05", 0.0, 0.05),
)


def smooth_weight(t: float) -> float:
    """Blender's "Smooth" proportional curve: 1 at t=0, 0 at t>=1."""
    return 1.0 - (3.0 * t * t - 2.0 * t * t * t)


def linear_weight(t: float) -> float:
    """Blender's "Linear" proportional curve: 1 - t."""
    return 1.0 - t


def sharp_weight(t: float) -> float:
    """Blender's "Sharp" proportional curve: (1 - t)^2."""
    return (1.0 - t) ** 2


def constant_weight(t: float) -> float:
    """Blender's "Constant" proportional curve: 1 everywhere inside radius."""
    return 1.0


_FALLOFF_CURVES = {
    "smooth": smooth_weight,
    "linear": linear_weight,
    "sharp": sharp_weight,
    "constant": constant_weight,
}


def falloff_weight(t: float, mode: str = DEFAULT_FALLOFF) -> float:
    """Return the scalar falloff weight for a normalised distance ``t``.

    ``t <= 0`` gives 1.0 and ``t >= 1`` gives 0.0 for every mode.
    """
    if mode not in _FALLOFF_CURVES:
        allowed = ", ".join(FALLOFF_MODES)
        raise ValueError(f"Unknown falloff mode {mode!r} (allowed: {allowed})")
    t = float(t)
    if t <= 0.0:
        return 1.0
    if t >= 1.0:
        return 0.0
    return float(_FALLOFF_CURVES[mode](t))


def _curve_values(t: np.ndarray, mode: str) -> np.ndarray:
    clipped = np.clip(t, 0.0, 1.0)
    if mode == "smooth":
        values = 1.0 - (3.0 * clipped**2 - 2.0 * clipped**3)
    elif mode == "linear":
        values = 1.0 - clipped
    elif mode == "sharp":
        values = (1.0 - clipped) ** 2
    else:
        values = np.ones_like(clipped)
    return np.where(t >= 1.0, 0.0, values)


def falloff_weights(
    coords: np.ndarray,
    member_indices: Sequence[int],
    radius_mm: float,
    mode: str = DEFAULT_FALLOFF,
) -> np.ndarray:
    """Return one falloff weight per vertex, measured from ORIGINAL positions.

    ``coords`` is ``(V, 3)`` object-local millimetres, ``member_indices`` are the
    probe vertices (weight forced to exactly 1.0), ``radius_mm`` is the
    proportional radius in millimetres. The result is constant across frames:
    callers must pass the undeformed base coordinates every time.
    """
    if mode not in _FALLOFF_CURVES:
        allowed = ", ".join(FALLOFF_MODES)
        raise ValueError(f"Unknown falloff mode {mode!r} (allowed: {allowed})")
    coords = np.asarray(coords, dtype=np.float64)
    if coords.ndim != 2 or coords.shape[1] != 3:
        raise ValueError(f"coords must have shape (V, 3), got {coords.shape}")
    members = np.asarray(list(member_indices), dtype=np.int64)
    if members.size == 0:
        raise ValueError("member_indices must not be empty")
    if members.min() < 0 or members.max() >= coords.shape[0]:
        raise ValueError("member_indices contains an out-of-range vertex index")
    radius_mm = float(radius_mm)
    if not radius_mm > 0.0:
        raise ValueError(f"radius_mm must be > 0, got {radius_mm!r}")

    deltas = coords[:, None, :] - coords[members][None, :, :]
    distance = np.sqrt((deltas**2).sum(axis=-1)).min(axis=1)
    weights = _curve_values(distance / radius_mm, mode)
    weights[members] = 1.0
    return weights


def affected_bands(weights: np.ndarray) -> dict[str, int]:
    """Histogram moved vertices by weight band; empty bands are omitted.

    Matches the ``affect_bands`` block of ``deform_report.json``: the measured
    Xynova default (radius 0.5 mm, smooth) is exactly
    ``{"1.0": 6, "0.3-0.5": 1, "0.05-0.1": 4}``.
    """
    values = np.asarray(weights, dtype=np.float64).ravel()
    bands: dict[str, int] = {}
    for label, low, high in _WEIGHT_BANDS:
        if label == "1.0":
            mask = values >= 1.0
        elif label == "0.0-0.05":
            mask = (values > 0.0) & (values < high)
        else:
            mask = (values >= low) & (values < high)
        count = int(mask.sum())
        if count:
            bands[label] = count
    return bands


def orthonormality_error(basis: np.ndarray) -> float:
    """Return max ``|basis @ basis.T - I|`` (0.0 for a perfect frame)."""
    matrix = np.asarray(basis, dtype=np.float64)
    if matrix.shape != (3, 3):
        raise ValueError(f"basis must have shape (3, 3), got {matrix.shape}")
    return float(np.abs(matrix @ matrix.T - np.eye(3)).max())


def orthonormal_basis(
    normals: np.ndarray,
    origin: Sequence[float] | None = None,
) -> tuple[np.ndarray | None, np.ndarray, dict[str, Any]]:
    """Build the probe-average orthonormal basis from probe vertex normals.

    Returns ``(origin, basis, report)``:

    * ``basis`` rows are ``x``, ``y``, ``z`` in that order, object-local;
    * ``z`` is the average of the unit normals (equal weight per vertex, so it
      is area-independent), ``x = normalize(cross((0,0,1), z))`` falling back to
      ``(1, 0, 0)`` when that is degenerate, ``y = cross(z, x)``;
    * ``origin`` is the probe centroid in object-local mm. Normals carry no
      position, so the caller must pass it (usually
      ``base[member_indices].mean(axis=0)``); when omitted, ``None`` is returned
      unchanged and ``report["origin_given"]`` is ``False``.

    ``report`` carries ``member_count``, ``normal_angle_max_deg``,
    ``orthonormality_error``, ``orthonormal`` and ``x_fallback``.
    """
    normals = np.asarray(normals, dtype=np.float64)
    if normals.ndim != 2 or normals.shape[1] != 3 or normals.shape[0] == 0:
        raise ValueError(f"normals must have shape (P, 3), got {normals.shape}")
    lengths = np.linalg.norm(normals, axis=1)
    if not np.all(lengths > 0.0):
        raise ValueError("probe vertex normals must be non-zero")
    unit = normals / lengths[:, None]

    mean_normal = unit.mean(axis=0)
    mean_length = float(np.linalg.norm(mean_normal))
    if mean_length <= 1e-12:
        raise ValueError("probe vertex normals cancel out; no surface normal")
    z = mean_normal / mean_length

    x = np.cross(np.array([0.0, 0.0, 1.0]), z)
    x_length = float(np.linalg.norm(x))
    x_fallback = x_length < 1e-9
    x = np.array([1.0, 0.0, 0.0]) if x_fallback else x / x_length
    y = np.cross(z, x)

    basis = np.vstack([x, y, z])
    angles = np.degrees(np.arccos(np.clip(unit @ z, -1.0, 1.0)))
    error = orthonormality_error(basis)
    report: dict[str, Any] = {
        "member_count": int(normals.shape[0]),
        "normal_angle_max_deg": float(angles.max()),
        "orthonormality_error": error,
        "orthonormal": error <= ORTHONORMAL_TOLERANCE,
        "x_fallback": bool(x_fallback),
        "origin_given": origin is not None,
    }
    origin_out = None if origin is None else np.asarray(origin, dtype=np.float64)
    return origin_out, basis, report


def flatness_mm(coords: np.ndarray, origin: Sequence[float], normal: Sequence[float]) -> float:
    """Peak-to-valley deviation of ``coords`` along the unit ``normal``, in mm.

    This is the contract's ``flatness_mm``: for the measured Xynova probe it is
    0.007611 mm (signed offsets along z span -0.00066..+0.00427 and
    -0.00334..+0.00369).
    """
    axis = np.asarray(normal, dtype=np.float64)
    axis_length = float(np.linalg.norm(axis))
    if axis_length <= 0.0:
        raise ValueError("normal must be non-zero")
    offsets = (np.asarray(coords, dtype=np.float64) - np.asarray(origin, dtype=np.float64)) @ (
        axis / axis_length
    )
    return float(offsets.max() - offsets.min())


def edge_lengths_mm(coords: np.ndarray, edges: np.ndarray) -> np.ndarray:
    """Return the length of every mesh edge, in object-local millimetres."""
    coords = np.asarray(coords, dtype=np.float64)
    edges = np.asarray(edges, dtype=np.int64).reshape(-1, 2)
    if edges.size == 0:
        return np.zeros(0, dtype=np.float64)
    return np.linalg.norm(coords[edges[:, 0]] - coords[edges[:, 1]], axis=1)


def percentile(values: np.ndarray, q: float) -> float:
    """Linear-interpolated percentile of a non-empty 1-D array (numpy default)."""
    values = np.asarray(values, dtype=np.float64).ravel()
    if values.size == 0:
        raise ValueError("percentile of an empty array is undefined")
    q = float(q)
    if not 0.0 <= q <= 100.0:
        raise ValueError(f"percentile q must be in [0, 100], got {q!r}")
    return float(np.percentile(values, q))