"""Hand L/R slotting: route raw hands to a fixed [left|None, right|None] pair. Chirality path (iPhone ARBodyTracker Vision): if `chirality` is aligned with `hands` (same length), route chir==0 -> slot 0 (left), chir==1 -> slot 1 (right). First match per slot wins; extras dropped. Fallback (chirality absent/misaligned): sort valid hands by screen cx ascending (flipped when mirror=True) and assign leftmost to slot 0, next to slot 1. `swap=True`: inverts the chirality INTERPRETATION only (chir 0 -> right slot, 1 -> left slot). Safety knob for a source delivering flipped chirality; ARBodyTracker chirality was validated CORRECT live 2026-07-02, so HAND_SWAP_LR defaults to 0 in the consumers. The cx fallback is screen-relative and is never swapped. `near_min > 0`: after routing, replace a slot with None when the hand's wrist-to-middle-MCP distance (normalised) is below `near_min`. """ from __future__ import annotations import math # ---- micro-helpers imported from hand_features (acyclic: hand_features only # imports hand_slots lazily inside step(), so a top-level import here is safe) --- from .hand_features import _finite, _coord, _clamp from .hand_display import hand_facing as _hand_facing def _hand_size(hand) -> float: """Wrist(0) -> middle-MCP(9) distance; works for both attr and list kp formats.""" w = hand[0] m = hand[9] wx = _finite(_coord(w, "x", 0), 0.5) wy = _finite(_coord(w, "y", 1), 0.5) mx = _finite(_coord(m, "x", 0), 0.5) my = _finite(_coord(m, "y", 1), 0.5) return math.hypot(mx - wx, my - wy) def _is_valid(hand) -> bool: try: return hand is not None and len(hand) >= 21 except TypeError: return False def route_hands( hands, chirality=None, *, mirror: bool = False, swap: bool = False, near_min: float = 0.0, ) -> list: # always [left|None, right|None] """Route hands to a 2-element [left, right] slot list. Parameters ---------- hands: raw hand list (any length, may contain None / short entries). chirality: int list aligned 1-to-1 with `hands` (0=left, 1=right). None / wrong length -> cx fallback. mirror: flip cx for the fallback path so screen-left stays slot 0. swap: invert the chirality mapping (0 -> right, 1 -> left). Safety knob for a source whose Vision chirality arrives inverted. Never touches the cx fallback (already screen-relative). near_min: replace a routed slot with None when hand_size < near_min. """ chir_aligned = ( chirality is not None and len(chirality) == len(hands) and len(chirality) > 0 ) # Build valid (chirality_value_or_None, hand) pairs — BEFORE filtering invalids # so chirality[i] stays paired with hands[i] even when hands[j] is skipped. valid_pairs: list[tuple] = [] for i, hand in enumerate(hands): if not _is_valid(hand): continue c = chirality[i] if chir_aligned else None valid_pairs.append((c, hand)) slots: list = [None, None] if chir_aligned and any(c is not None for c, _ in valid_pairs): # ---- Chirality path: chir 0 -> slot 0 (left), 1 -> slot 1 (right); # swap inverts the mapping when the source chirality is flipped ---- for c, hand in valid_pairs: target = c if c in (0, 1) else None if target is not None and swap: target = 1 - target if target is not None and slots[target] is None: slots[target] = hand else: # ---- cx fallback: leftmost on screen -> slot 0 ---- def _cx(h): xs = [_finite(_coord(p, "x", 0), 0.5) for p in h[:21]] raw = _clamp(sum(xs) / len(xs), 0.0, 1.0) return (1.0 - raw) if mirror else raw ordered = sorted((h for _, h in valid_pairs), key=_cx) if ordered: slots[0] = ordered[0] if len(ordered) >= 2: slots[1] = ordered[1] # ---- near_min gate (applied after routing, both paths) ---- if near_min > 0.0: for i in range(2): if slots[i] is not None and _hand_size(slots[i]) < near_min: slots[i] = None return slots class GestureSlotStabilizer: """Stateful per-slot stabilizer for the gesture detection path. Applied AFTER route_hands (with near_min=0.0) and BEFORE the gesture detectors (FingerStrikeDetector, PinchDetector). Two behaviours: - **Hold-last**: when a slot is None this call but held a hand within the last ``hold_frames`` calls, output the LAST seen hand for that slot. Fills 1-2 frame Vision holes so pinch debounce never sees a spurious absence. After ``hold_frames`` consecutive misses the slot yields None for real. - **Near hysteresis**: a slot ACTIVATES when hand_size >= ``near_on`` and stays active until hand_size < ``near_off``. Inactive slot -> None output (but hold-last still tracks so re-activation is instant). """ def __init__( self, hold_frames: int = 2, near_on: float = 0.10, near_off: float = 0.08, face_min: float = 0.5, ) -> None: self._hold = hold_frames self._near_on = near_on self._near_off = near_off self._face_min = face_min # face_off uses 0.8× hysteresis (mirrors the near on/off pattern) so that # a slot active at face_min does not flap when facing oscillates near the # threshold — it only deactivates when facing drops below face_off. self._face_off = 0.8 * face_min # Per-slot state self._last: list = [None, None] # last seen hand object self._miss: list[int] = [0, 0] # consecutive miss count self._near_active: list[bool] = [False, False] self._face_active: list[bool] = [False, False] self._out: list = [None, None] # last step() output (for active_flags) # Hold-tracking: True when previous step() output was a held replay. self._held_flag: list[bool] = [False, False] # Resumed: True when this step() transitioned held -> real for that slot. self._resumed: list[bool] = [False, False] def step(self, slotted: list) -> list: """Stabilize a 2-element [left|None, right|None] slot list. Args: slotted: output of route_hands (near_min=0.0) — length 2. Returns: Stabilized 2-element list; each slot is the stabilized hand or None. """ out: list = [None, None] for i in range(2): hand = slotted[i] if i < len(slotted) else None prev_held = self._held_flag[i] if hand is not None: # Real hand present this frame. # Transition held -> real: mark resumed so callers can reset # per-finger state before consuming this step's output. self._resumed[i] = prev_held self._held_flag[i] = False # Update near hysteresis size = _hand_size(hand) if self._near_active[i]: if size < self._near_off: self._near_active[i] = False else: if size >= self._near_on: self._near_active[i] = True # Update face hysteresis (face_off = 0.8 * face_min) facing = _hand_facing(hand) if self._face_active[i]: if facing < self._face_off: self._face_active[i] = False else: if facing >= self._face_min: self._face_active[i] = True # Track the hand regardless of near/face state self._last[i] = hand self._miss[i] = 0 # Output only if near-active AND facing the camera out[i] = hand if (self._near_active[i] and self._face_active[i]) else None else: # Hand absent this frame self._resumed[i] = False self._miss[i] += 1 if self._miss[i] <= self._hold: # Hold: output last hand only if it was near+face-active if (self._near_active[i] and self._face_active[i] and self._last[i] is not None): self._held_flag[i] = True out[i] = self._last[i] else: # Not active: no hold replay; slot stays None self._held_flag[i] = False else: # Past hold limit — clear both gates so next appearance # must re-qualify from near_on and face_min. self._held_flag[i] = False self._near_active[i] = False self._face_active[i] = False out[i] = None self._out = out return out def active_flags(self) -> tuple: """Return (slot0_active, slot1_active). True when the last step() yielded a non-None for that slot — meaning the hand is established (held or present) AND within the near zone. Used by action_head_pub._emit_hands to gate the fist feature. """ return (self._out[0] is not None, self._out[1] is not None) def resumed_flags(self) -> tuple[bool, bool]: """Return (slot0_resumed, slot1_resumed). True for a slot when the current step() transitioned from a "held replay" (Vision hole, last hand replayed) back to a real hand. Callers that track per-frame deltas (e.g. FingerStrikeDetector) should reset their state for that slot BEFORE consuming the step output to avoid phantom events caused by the compressed motion accumulated during the hold window. Semantics: True only for the ONE step where the transition occurs; False on all subsequent frames where the hand stays real, and False after an expired hold (miss > hold_frames, slot went None) since the absent-slot logic in gesture detectors already resets per-finger state. """ return (self._resumed[0], self._resumed[1])