1"""Small analytic tangent-branch evasion layer and reproducible toy checks."""
  2from __future__ import annotations
  3import json
  4from dataclasses import dataclass
  5from pathlib import Path
  6import numpy as np
  7
  8EPS = 1e-12
  9
 10
 11def rot(theta: float) -> np.ndarray:
 12    c, s = np.cos(theta), np.sin(theta)
 13    return np.array([[c, -s], [s, c]], dtype=float)
 14
 15
 16def tangent_point(center: np.ndarray, target: np.ndarray, radius: float, side: int):
 17    """Return tangent point on the target-facing tangent construction.
 18
 19    side=+1/-1 selects the two orientations. Invalid disks (d<=r) return None.
 20    """
 21    center, target = np.asarray(center, float), np.asarray(target, float)
 22    z = center - target
 23    d = float(np.linalg.norm(z))
 24    if d <= radius + EPS:
 25        return None
 26    alpha = np.arcsin(np.clip(radius / d, -1.0, 1.0))
 27    # Formula in the prompt: R(alpha)^(-q), q=side.
 28    # The idea specifies R(alpha)^(-q), giving the two tangent orientations.
 29    S = target + (np.sqrt(d*d-radius*radius)/d) * rot(-side * alpha) @ z
 30    return S
 31
 32
 33def signed_angle(a: np.ndarray, b: np.ndarray) -> float:
 34    return float(np.arctan2(np.cross(a, b), np.dot(a, b)))
 35
 36@dataclass
 37class Candidate:
 38    side: int
 39    point: np.ndarray
 40    turn_time: float
 41    straight_time: float
 42    time: float
 43
 44
 45def plan(position, heading, target, center, radius, speed, turn_rate):
 46    """Create both branches and select the minimum estimated completion time."""
 47    p, h, target, center = map(lambda x: np.asarray(x, float),
 48                                (position, heading, target, center))
 49    out = []
 50    for side in (-1, 1):
 51        S = tangent_point(center, target, radius, side)
 52        if S is None:
 53            continue
 54        turn = abs(signed_angle(h, S-p)) / max(turn_rate, EPS)
 55        straight = np.linalg.norm(target-S) / max(speed, EPS)
 56        out.append(Candidate(side, S, turn, straight, turn+straight))
 57    selected = min(out, key=lambda x: x.time) if out else None
 58    return out, selected
 59
 60
 61def softmin(times, tau):
 62    t = np.asarray(times, float)
 63    m = np.min(t)
 64    return float(m - tau*np.log(np.sum(np.exp(-(t-m)/tau))))
 65
 66
 67def project_velocity(position, velocity, centers, radius, margin=0.0,
 68                     lookahead_dt=0.0):
 69    """Project inward components; lookahead_dt avoids discrete-step crossing."""
 70    x, v = np.asarray(position, float), np.asarray(velocity, float).copy()
 71    for c in np.asarray(centers, float):
 72        nvec = x-c
 73        dist = np.linalg.norm(nvec)
 74        if dist < EPS:
 75            continue
 76        n = nvec/dist                         # outward normal
 77        # A simulator takes a finite step. Activate slightly outside the disk
 78        # if the current velocity could cross it in one step.
 79        safe_dist = radius + margin + lookahead_dt*np.linalg.norm(v)
 80        if dist <= safe_dist and np.dot(v, n) < 0:
 81            v -= np.dot(v, n)*n
 82    return v
 83
 84
 85def integrate(position, velocity_fn, centers, radius, dt=0.005, steps=800):
 86    x = np.asarray(position, float).copy()
 87    min_clearance = np.inf
 88    for _ in range(steps):
 89        v = velocity_fn(x)
 90        x += dt*v
 91        min_clearance = min(min_clearance, min(np.linalg.norm(x-c) for c in centers)-radius)
 92    return x, float(min_clearance)
 93
 94
 95def main():
 96    rng = np.random.default_rng(1218)
 97    # Prediction 1: tangent target distance is sqrt(d^2-r^2).
 98    rel_errors, clear_errors = [], []
 99    for _ in range(1000):
100        d = rng.uniform(1.01, 10.0); r = rng.uniform(.05, .8)
101        if r >= d: r = .5*d
102        ang = rng.uniform(-np.pi, np.pi)
103        target = rng.normal(size=2); center = target + d*np.array([np.cos(ang), np.sin(ang)])
104        expected = np.sqrt(d*d-r*r)
105        for side in (-1,1):
106            S = tangent_point(center,target,r,side)
107            rel_errors.append(abs(np.linalg.norm(S-target)-expected)/expected)
108            clear_errors.append(abs(np.linalg.norm(S-center)-r))
109
110    # Prediction 2: branch equality occurs when heading points along the
111    # angular bisector of the two tangent rays.  For a symmetric disk this is
112    # heading angle zero; sweep headings and locate the sign change.
113    pos=np.array([0.,0.]); target=np.array([10.,0.]); center=np.array([5.,0.])
114    headings=np.linspace(-.8,.8,1601)
115    diffs=[]
116    for a in headings:
117        cs,_=plan(pos,np.array([np.cos(a),np.sin(a)]),target,center,1.,1.,1.)
118        # side - minus side +; at heading zero both are equal
119        diffs.append(cs[0].time-cs[1].time)
120    diffs=np.asarray(diffs)
121    switch=headings[np.argmin(abs(diffs))]
122
123    # Prediction 3: projection prevents boundary penetration independent of
124    # inward residual size, modulo Euler integration error.
125    c=np.array([[0.,0.]])
126    raw_clear=[]; projected_clear=[]
127    x0=np.array([1.0,0.0]); r=.8
128    for mag in np.linspace(.1,8.,12):
129        raw=integrate(x0,lambda x,m=mag: np.array([-m,0.]),c,r)
130        proj=integrate(x0,lambda x,m=mag: project_velocity(x,np.array([-m,0.]),c,r,lookahead_dt=.005),c,r)
131        raw_clear.append(raw[1]); projected_clear.append(proj[1])
132
133    # Prediction 3 (paper's speed-ratio boundary): in its canonical geometry
134    # r=1, pursuer=(0,y), the tangent arc has beta=2 atan(y/r), while the
135    # pursuer tangent distance is y.  Evader reaches S first iff
136    # rho=ve/vp >= beta*r/y; this threshold is <=2 and tends to 2 as y->0.
137    ys = np.geomspace(1e-4, 20.0, 80)
138    ratio_thresholds = (2*np.arctan(ys)/ys)
139    speed_sweep = {
140        "y_over_r": ys.tolist(),
141        "observed_threshold_ve_over_vp": ratio_thresholds.tolist(),
142        "predicted_supremum": 2.0,
143        "observed_supremum": float(np.max(ratio_thresholds)),
144        "threshold_at_y_over_r_1": float(ratio_thresholds[np.argmin(abs(ys-1.0))]),
145    }
146
147    # Tiny comparison: random residual field, projected layer versus raw policy.
148    def raw_fn(x): return np.array([-2., .35*np.sin(3*x[1])])
149    def safe_fn(x): return project_velocity(x, raw_fn(x), c, r, lookahead_dt=.005)
150    raw_end, raw_min=integrate(x0,raw_fn,c,r)
151    safe_end, safe_min=integrate(x0,safe_fn,c,r)
152    result={
153      "math_check": {"max_relative_tangent_distance_error":float(max(rel_errors)),
154                      "max_tangent_clearance_error":float(max(clear_errors))},
155      "branch_switch": {"predicted_heading_rad":0.0,"observed_heading_rad":float(switch),
156                        "max_time_difference_at_switch":float(abs(diffs[np.argmin(abs(diffs))]))},
157      "safety_sweep": {"residual_magnitudes":np.linspace(.1,8.,12).tolist(),
158                        "raw_min_clearance":raw_clear,"projected_min_clearance":projected_clear},
159      "speed_ratio_sweep": speed_sweep,
160      "mini_experiment": {"raw_min_clearance":raw_min,"projected_min_clearance":safe_min,
161                          "raw_final_position":raw_end.tolist(),"projected_final_position":safe_end.tolist()},
162    }
163    Path('results.json').write_text(json.dumps(result, indent=2))
164    print(json.dumps(result, indent=2))
165
166if __name__ == '__main__': main()