cooptools 1.63__tar.gz → 1.65__tar.gz

{cooptools-1.63 → cooptools-1.65}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: cooptools
-Version: 1.63
+Version: 1.65
 Summary: Generic Tooling
 Home-page: https://github.com/tylertjburns/cooptools
 Author: tburns

{cooptools-1.63 → cooptools-1.65}/cooptools/decor.py

@@ -2,7 +2,8 @@ import functools
 import time
 import logging
 import inspect
-from typing import Protocol, Dict
+import threading
+from typing import Protocol, Dict, Optional
 from typing_extensions import runtime_checkable
 
 @runtime_checkable
@@ -15,6 +16,22 @@ class TimeableProtocol(Protocol):
 
 TIMEABLE_LOOKUPATTR = "_timeable"
 
+# ---------------------------------------------------------------------------
+# Thread-local call stack — tracks the currently executing @timer chain so
+# each invocation knows its parent function.  Push before calling func, pop
+# after (even on exception) so the stack stays consistent.
+# ---------------------------------------------------------------------------
+
+_call_stack_local = threading.local()
+
+
+def get_call_stack() -> list:
+    """Return the current thread's timer call stack (mutable live reference)."""
+    if not hasattr(_call_stack_local, 'stack'):
+        _call_stack_local.stack = []
+    return _call_stack_local.stack
+
+
 def try_handler(logger: logging.Logger = None):
     def wrapper(func):
         @functools.wraps(func)
@@ -32,7 +49,8 @@ def try_handler(logger: logging.Logger = None):
     return wrapper
 
 
-def update_ttc_values(ttc: TimeableProtocol, func_name: str, run_time: float):
+def update_ttc_values(ttc: TimeableProtocol, func_name: str, run_time: float,
+                      parent: Optional[str] = None):
     #update counter
     ttc.ns.setdefault(func_name, 0)
     ttc.ns[func_name] += 1
@@ -55,6 +73,10 @@ def update_ttc_values(ttc: TimeableProtocol, func_name: str, run_time: float):
     my_avg = ttc.internally_tracked_times[func_name] / ttc.ns[func_name]
     ttc.avgs[func_name] = my_avg
 
+    # Record call-chain parent once (hierarchy is static; first writer wins).
+    if parent is not None and hasattr(ttc, 'parents'):
+        ttc.parents.setdefault(func_name, parent)
+
 
 def timer(logger: logging.Logger = None,
           log_level: int = logging.DEBUG,
@@ -68,11 +90,20 @@ def timer(logger: logging.Logger = None,
 
         @functools.wraps(func)
         def wrapper_timer(*args, **kwargs):
-            # capture time
+            # Push to call stack before executing so nested @timer calls see us
+            # as their parent.  Pop in finally so the stack stays consistent
+            # even if func raises.
+            _stack = get_call_stack()
+            _parent: Optional[str] = _stack[-1] if _stack else None
+            _stack.append(func.__name__)
+
             start_time = time.perf_counter()  # 1
-            value = func(*args, **kwargs)
-            end_time = time.perf_counter()  # 2
-            run_time = end_time - start_time  # 3
+            try:
+                value = func(*args, **kwargs)
+            finally:
+                end_time = time.perf_counter()  # 2
+                run_time = end_time - start_time  # 3
+                _stack.pop()
 
             ttc = None
 
@@ -96,7 +127,8 @@ def timer(logger: logging.Logger = None,
             if ttc is None:
                 return value
 
-            update_ttc_values(ttc, func_name=func.__name__, run_time=run_time)
+            update_ttc_values(ttc, func_name=func.__name__, run_time=run_time,
+                              parent=_parent)
             end_time2 = time.perf_counter()
             timer_runtime = end_time2 - end_time
             update_ttc_values(ttc, func_name=TIMEABLE_LOOKUPATTR, run_time=timer_runtime)

{cooptools-1.63 → cooptools-1.65}/cooptools/geometry_utils/circle_utils.py

@@ -1,4 +1,5 @@
-from typing import Tuple
+from dataclasses import dataclass
+from typing import Tuple, Dict
 from cooptools.common import verify_unique, verify_len, rads_to_degrees, degree_to_rads
 from cooptools.geometry_utils.vector_utils import interpolate, orthogonal2x2, vector_between, add_vectors, \
     distance_between, zero_vector
@@ -8,8 +9,48 @@ import cooptools.geometry_utils.line_utils as line
 from cooptools.geometry_utils import common as cmn
 from cooptools.geometry_utils.common import CircleCenterRadius
 import math
+import numpy as np
 import random as rnd
-from typing import Dict
+
+
+@dataclass
+class CircleTrajectoryConstants:
+    """
+    Pre-computed l-independent geometric constants for one circle-arc trajectory variant.
+
+    Describes a three-segment path:
+        x0 -> x1   straight,       length = l
+        x1 -> x2   circular arc,   length = r(l) * theta
+        x2 -> xT   straight,       length = |xT - x2(l)|
+
+    All fields are derived from fixed geometry (positions and headings) and are
+    independent of l, so they are computed once and reused for any value of l.
+
+    Radius (piecewise-linear in l):
+        t(l) = A + B*l        signed perpendicular offset from x1 to the circle center
+        r(l) = |t(l)|         circle radius
+
+    Exit point (affine in l):
+        x2(l) = p + l*q       p and q are constant 2D vectors
+
+    Final segment length (square-root quadratic in l):
+        f            = xT - p
+        |xT - x2(l)| = |f - l*q| = sqrt(f_sq - 2*l*f_dot_q + l^2*q_sq)
+
+    Arc angles (always sum to 2*pi):
+        theta_direct:     CCW arc from x1 to x2
+        theta_complement: CW  arc from x1 to x2
+    """
+    A: float            # constant offset in t(l) = A + B*l
+    B: float            # slope         in t(l) = A + B*l
+    theta_direct: float       # CCW arc angle [radians]
+    theta_complement: float   # CW  arc angle [radians]
+    p: np.ndarray       # x2(l=0), base point of the exit position
+    q: np.ndarray       # dx2/dl, direction the exit point moves as l increases
+    f: np.ndarray       # xT - p, constant vector used for the final segment
+    f_dot_q: float      # dot(f, q), precomputed
+    q_sq: float         # |q|^2,    precomputed
+    f_sq: float         # |f|^2,    precomputed
 
 
 def from_boundary_points(a: Tuple[float, float], b: Tuple[float, float], c: Tuple[float, float]) -> CircleCenterRadius:
@@ -104,6 +145,121 @@ def rotated_point(point: cmn.FloatVec,
     return x2
 
 
+def circle_constants_for_bisector(
+    x0: vec.FloatVec,
+    xT: vec.FloatVec,
+    O: vec.FloatVec,
+    u0: vec.FloatVec,
+    u0_perp: vec.FloatVec,
+    u_b: vec.FloatVec,
+    rotation_rads: float,
+) -> CircleTrajectoryConstants:
+    """
+    Derive closed-form constants that express circle radius and exit point as functions of l.
+
+    Context:
+        A trajectory enters at x0 with unit heading u0, travels a straight distance l to
+        x1 = x0 + l*u0, then follows a circular arc to exit point x2, then travels in a
+        straight line to xT.  The circle center C lies on both:
+          (1) the perpendicular to u0 through x1, and
+          (2) a bisector ray from O in direction u_b.
+
+    This function converts the geometry of finding that circle (normally done by intersecting
+    two lines) into linear algebraic coefficients so that r(l) and x2(l) can be evaluated
+    directly without recomputing intersections.
+
+    DERIVING t(l) — signed offset from x1 to the circle center along u0_perp:
+
+        C must satisfy two parametric equations simultaneously:
+          C = x1 + t * u0_perp        (perpendicular through x1)
+          C = O  + s * u_b            (bisector ray from O)
+
+        Substituting x1 = x0 + l*u0 and setting the two equal:
+            x0 + l*u0 + t*u0_perp = O + s*u_b
+
+        Take the 2D cross product of both sides with u_b (eliminates s, since u_b x u_b = 0):
+            (x0 - O) x u_b  +  l*(u0 x u_b)  +  t*(u0_perp x u_b)  =  0
+
+        Solve for t:
+            t(l) = [ (O - x0) x u_b  -  l*(u0 x u_b) ]  /  (u0_perp x u_b)
+                 = A + B*l
+
+        Where (D = u0_perp x u_b as the shared denominator):
+            A = (O - x0) x u_b  / D      (constant offset — value of t when l=0)
+            B = -(u0 x u_b)     / D      (slope — rate of change of t with l)
+
+        The radius r(l) = |t(l)| is piecewise-linear in l, with a kink at t=0.
+
+    DERIVING x2(l) — the arc exit point:
+
+        Since C = x1 + t*u0_perp and x2 is x1 rotated around C by rotation_rads:
+            x2 = C + R(rotation_rads) * (x1 - C)
+               = C - t * R(rotation_rads) * u0_perp      [x1 - C = -t * u0_perp]
+               = x1 + t * u0_perp - t * R * u0_perp
+               = x1 + t * (u0_perp - R * u0_perp)
+
+        Substituting x1 = x0 + l*u0  and  t = A + B*l:
+            x2(l) = x0 + l*u0 + (A + B*l) * w    where w = u0_perp - R(rotation_rads)*u0_perp
+                  = (x0 + A*w) + l*(u0 + B*w)
+                  = p + l*q
+
+        This affine-in-l result means x2 slides along a straight line as l varies —
+        a key structural property that makes the full trajectory length analytically tractable.
+
+    Args:
+        x0:            Initial position (start of trajectory).
+        xT:            Target position (end of trajectory).
+        O:             Intersection point of the two heading rays.
+        u0:            Unit initial heading vector.
+        u0_perp:       Vector perpendicular to u0 (any 90-degree rotation; sign does not affect result).
+        u_b:           Unit direction of the bisector ray from O (determines which circle family).
+        rotation_rads: CCW rotation angle [radians] applied to x1 around C to reach x2.
+
+    Returns:
+        CircleTrajectoryConstants with all l-independent coefficients precomputed.
+    """
+    x0_np = np.array(x0, dtype=float)
+    xT_np = np.array(xT, dtype=float)
+    O_np  = np.array(O,  dtype=float)
+    u0_np = np.array(u0, dtype=float)
+    u0_perp_np = np.array(u0_perp, dtype=float)
+    u_b_np = np.array(u_b, dtype=float)
+
+    # 2D cross product: a x b = a[0]*b[1] - a[1]*b[0]  (scalar z-component).
+    # Computed manually rather than via np.cross, which dropped support for
+    # 2D input vectors (deprecated in NumPy 2.0, removed in later 2.x releases).
+    def _cross_2d(a: np.ndarray, b: np.ndarray) -> float:
+        return float(a[0] * b[1] - a[1] * b[0])
+
+    D = _cross_2d(u0_perp_np, u_b_np)           # denominator, constant
+    A = _cross_2d(O_np - x0_np, u_b_np) / D     # t-offset when l=0
+    B = -_cross_2d(u0_np, u_b_np) / D           # dt/dl
+
+    # w encodes how the exit point shifts relative to t (and therefore l).
+    # rotated_point with default center=(0,0) applies a pure vector rotation.
+    rotated_u0_perp = np.array(rotated_point(tuple(u0_perp_np), rads=rotation_rads))
+    w = u0_perp_np - rotated_u0_perp   # constant 2D vector: u0_perp - R(rotation_rads)*u0_perp
+
+    p = x0_np + A * w      # x2(l=0)
+    q = u0_np + B * w      # dx2/dl
+    f = xT_np - p          # constant vector: |xT - x2(l)| = |f - l*q|
+
+    # CCW arc from x1 to x2 subtends rotation_rads mod 2*pi;
+    # CW arc takes the remainder to complete the full circle.
+    theta_direct     = rotation_rads % (2 * math.pi)
+    theta_complement = 2 * math.pi - theta_direct
+
+    return CircleTrajectoryConstants(
+        A=A, B=B,
+        theta_direct=theta_direct,
+        theta_complement=theta_complement,
+        p=p, q=q, f=f,
+        f_dot_q=float(np.dot(f, q)),
+        q_sq=float(np.dot(q, q)),
+        f_sq=float(np.dot(f, f)),
+    )
+
+
 def random_point_on_circle(center: vec.FloatVec, radius: float) -> vec.FloatVec:
     rads = rnd.uniform(0, math.pi * 2)
     return point_at_angle(center, radius, radians=rads)

{cooptools-1.63 → cooptools-1.65}/cooptools/graphs/graph.py

@@ -15,6 +15,7 @@ from cooptools.sectors import sectorTree as st
 from cooptools.graphs import utils as u
 from dataclasses import dataclass
 from functools import lru_cache
+import heapq
 import json
 from cooptools.graphs.graph_dcs import *
 
@@ -663,7 +664,8 @@ class Graph(object):
               h_func: Callable[[Node, Node], float] = None,
               ignored_disablers:frozenset[str]=None,
               disabled_node_ids:frozenset[str] = None,
-              early_out_cost: float = None) -> AStarResults:
+              early_out_cost: float = None,
+              collect_steps: bool = False) -> AStarResults:
 
         if disabled_node_ids is not None:
             if ((s_in:=(start.name in disabled_node_ids)) or
@@ -684,30 +686,37 @@ class Graph(object):
 
         steps = {}
 
-        # Initialize both open and closed list
-        open_set = set()
+        # open_heap: min-heap of (f, counter, AStarMetrics) — O(log N) extraction
+        # open_nodes: {node_name -> AStarMetrics} — O(1) lookup; authoritative membership
+        # Lazy deletion: heap entries whose node_name is no longer in open_nodes are stale and skipped.
+        open_heap = []
+        open_nodes = {}
         closed_set = set()
+        _heap_counter = 0
 
         # Check reachability: any enabled edge into end whose source isn't disabled
         connections_to_end = self.edges_to_node(end, only_enabled=True, ignored_disablers=ignored_disablers)
         if disabled_node_ids is not None:
             connections_to_end = [e for e in connections_to_end if e.start.name not in disabled_node_ids]
         if len(connections_to_end) != 0:
-            open_set.add(start_iter)
+            heapq.heappush(open_heap, (0.0, _heap_counter, start_iter))
+            open_nodes[start_iter.graph_node.name] = start_iter
 
         cc = -1
 
         results = None
         _early_out_triggered = False
         # Loop until you find the end
-        while len(open_set) > 0:
+        while open_heap:
             cc += 1
 
-            # Find the node on open list with the least F value
-            current_item = next(iter(open_set))
-            for open_item in open_set:
-                if open_item.f < current_item.f:
-                    current_item = open_item
+            _, _, current_item = heapq.heappop(open_heap)
+
+            # Lazy deletion: skip heap entries superseded by a better path
+            if open_nodes.get(current_item.graph_node.name) is not current_item:
+                continue
+
+            del open_nodes[current_item.graph_node.name]
 
             # Early-out: f is a lower bound on total path cost through current_item.
             # If the minimum f in the open set already meets or exceeds the caller's
@@ -716,10 +725,9 @@ class Graph(object):
                 _early_out_triggered = True
                 break
 
-            steps[cc] = {"open_set": set(open_set), "closed_set": set(closed_set), "current_item": current_item}
+            if collect_steps:
+                steps[cc] = {"open_set": set(open_nodes.values()), "closed_set": set(closed_set), "current_item": current_item}
 
-            # Pop current off open list, add to closed list
-            open_set.remove(current_item)
             closed_set.add(current_item)
 
             # Found the goal
@@ -765,19 +773,26 @@ class Graph(object):
                 if new_item in closed_set:
                     continue
 
-                open_set.add(new_item)
-
-                # calculate new g, h, f from current pivot node to the new node
+                # calculate new g, h, f before deciding whether to push
                 calc_g = current_item.g + (g_func(current_item.graph_node, new_node) if g_func else min_edge.cost)
                 calc_h = h_func(new_node,
                                 end_iter.graph_node) if h_func else vec.distance_between(new_node.pos, end_iter.graph_node.pos)
                 calc_f = calc_g + calc_h
 
+                # Skip if we already have a path to this node that's at least as good
+                existing = open_nodes.get(new_node.name)
+                if existing is not None and existing.g <= calc_g:
+                    continue
+
                 new_item.parent = current_item
                 new_item.g = calc_g
                 new_item.h = calc_h
                 new_item.f = calc_f
 
+                _heap_counter += 1
+                heapq.heappush(open_heap, (calc_f, _heap_counter, new_item))
+                open_nodes[new_node.name] = new_item
+
         if results is None:
             if _early_out_triggered:
                 logger.debug(f"A* early-out from [{start}] to [{end}]: f >= early_out_cost ({early_out_cost})")

cooptools-1.65/cooptools/physics/demo/server.py

@@ -0,0 +1,136 @@
+"""
+Local demo server for GoalSeeker / WaypointFollower.
+
+Runs the *real* cooptools.physics classes and returns recorded frame-by-frame
+trajectories over HTTP so the accompanying index.html can replay genuine output --
+no physics is reimplemented in JavaScript.
+
+Usage (from the repo root, with cooptools importable):
+    python -m cooptools.physics.demo.server
+    (or) python cooptools/physics/demo/server.py
+
+Then open the printed URL (defaults to http://127.0.0.1:8765).
+"""
+import json
+import http.server
+import socketserver
+import webbrowser
+from pathlib import Path
+
+from cooptools.physics.waypointFollower import WaypointFollower
+from cooptools.physics.waypoint import Waypoint
+from cooptools.physics.movement import Velocity
+from cooptools.transform import Transform
+
+DEMO_DIR = Path(__file__).parent
+INDEX_FILE = DEMO_DIR / "index.html"
+
+
+def run_simulation(payload: dict) -> dict:
+    start_pos = tuple(payload["start_pos"])
+    start_vel = tuple(payload.get("start_vel", (0.0, 0.0)))
+    max_accel = float(payload["max_accel"])
+    realign = bool(payload.get("realign", True))
+    dt_ms = int(payload.get("dt_ms", 16))
+    max_steps = int(payload.get("max_steps", 20000))
+    # GoalSeeker's library defaults (0.001) assume a much smaller unit scale than
+    # canvas pixels, and simultaneous multi-axis arrivals (a diagonal destination)
+    # only settle to a small bounded residual, not an arbitrarily tight one -- see
+    # arrival_acceleration's docstring. A couple of pixels is a perfectly "arrived"
+    # demo and avoids waiting on a residual that may never fully vanish.
+    close_enough_tolerance = float(payload.get("close_enough_tolerance", 2.0))
+    slow_enough_tolerance = float(payload.get("slow_enough_tolerance", 2.0))
+
+    follower = WaypointFollower(
+        name="demo",
+        initial_transform=Transform(translation=start_pos),
+        max_acceleration=max_accel,
+        initial_velocity=Velocity(initial_m_s_vec=start_vel),
+    )
+    follower.add_waypoints([
+        Waypoint(end_pos=tuple(wp["pos"]), is_destination=bool(wp.get("is_destination", False)))
+        for wp in payload["waypoints"]
+    ])
+
+    def snapshot(t_ms, reached=None, target_was_destination=None):
+        return {
+            "t_ms": t_ms,
+            "pos": list(follower.Position),
+            "vel": list(follower.Velocity.CurrentVelocity_M_S),
+            "acc": list(follower.Acceleration.CurrentAccel_M_S),
+            "reached": [{"pos": list(w.end_pos), "is_destination": w.is_destination} for w in (reached or [])],
+            "target_was_destination": target_was_destination,
+        }
+
+    frames = [snapshot(0)]
+    t_ms = 0
+    for _ in range(max_steps):
+        if not follower.waypoints:
+            break
+        target_was_destination = follower.CurrentTarget.is_destination
+        reached = follower.update(
+            delta_time_ms=dt_ms,
+            realign_to_next_waypoint=realign,
+            close_enough_tolerance=close_enough_tolerance,
+            slow_enough_tolerance=slow_enough_tolerance,
+        )
+        t_ms += dt_ms
+        frames.append(snapshot(t_ms, reached, target_was_destination))
+
+    return {"frames": frames, "completed": len(follower.waypoints) == 0}
+
+
+class DemoHandler(http.server.BaseHTTPRequestHandler):
+    def _send_json(self, status: int, payload: dict):
+        body = json.dumps(payload).encode("utf-8")
+        self.send_response(status)
+        self.send_header("Content-Type", "application/json")
+        self.send_header("Content-Length", str(len(body)))
+        self.end_headers()
+        self.wfile.write(body)
+
+    def do_GET(self):
+        if self.path in ("/", "/index.html"):
+            body = INDEX_FILE.read_bytes()
+            self.send_response(200)
+            self.send_header("Content-Type", "text/html; charset=utf-8")
+            self.send_header("Content-Length", str(len(body)))
+            self.end_headers()
+            self.wfile.write(body)
+        else:
+            self.send_error(404)
+
+    def do_POST(self):
+        if self.path != "/simulate":
+            self.send_error(404)
+            return
+        try:
+            length = int(self.headers.get("Content-Length", 0))
+            payload = json.loads(self.rfile.read(length))
+            result = run_simulation(payload)
+            self._send_json(200, result)
+        except Exception as e:
+            self._send_json(400, {"error": str(e)})
+
+    def log_message(self, format, *args):
+        pass  # quiet console; the simulation output is the point, not access logs
+
+
+class _Server(socketserver.ThreadingTCPServer):
+    allow_reuse_address = True
+
+
+def main(port: int = 8765):
+    with _Server(("127.0.0.1", port), DemoHandler) as httpd:
+        url = f"http://127.0.0.1:{port}"
+        print(f"Seek/Arrive demo server running at {url}")
+        print("Ctrl+C to stop.")
+        webbrowser.open(url)
+        try:
+            httpd.serve_forever()
+        except KeyboardInterrupt:
+            pass
+
+
+if __name__ == "__main__":
+    main()

{cooptools-1.63 → cooptools-1.65}/cooptools/physics/kinematic.py

@@ -203,11 +203,13 @@ class GoalSeeker:
                close_enough_tolerance: float = 0.001,
                slow_enough_tolerance: float = 0.001,
                stop_rotating_threshold: int = 10,
-               velocity_magnitude_bounds=None
+               velocity_magnitude_bounds=None,
+               continue_after_goal_reached: bool = False,
+               decel_lookahead_distance: float = None
                ) -> bool:
         verify_val(delta_time_ms, gte=0, not_none=True)
 
-        self._last_pos = self._mover.Transform.Translation.Vector
+        last_pos = self._mover.Transform.Translation.Vector
         self._mover.update(delta_time_ms=delta_time_ms,
                            stop_rotating_threshold=stop_rotating_threshold,
                            velocity_magnitude_bounds=velocity_magnitude_bounds)
@@ -219,33 +221,109 @@ class GoalSeeker:
         elif self._goal is None:
             self._goal = self._mover.Transform.Translation.Vector
 
-        ''' Calculate Vector to goal'''
-        vector_to_goal = vec.vector_between(self._mover.Transform.Translation.Vector, self._goal)
+        current_pos = self._mover.Transform.Translation.Vector
 
-        ''' Return early once "close enough"'''
-        close = vec.vector_len(vector_to_goal) <= close_enough_tolerance
+        ''' Calculate Vector to goal'''
+        vector_to_goal = vec.vector_between(current_pos, self._goal)
+
+        ''' "Close enough" is checked against the closest point on the segment traveled
+        this update (last_pos -> current_pos), not just the endpoint. Otherwise a large
+        delta_time_ms can carry the mover clean past the goal in a single step without
+        either endpoint ever landing inside close_enough_tolerance, silently missing it. '''
+        projected_closest = vec.project_onto(a=self._goal, b=current_pos, origin=last_pos)
+        if vec.bounded_by(projected_closest, last_pos, current_pos):
+            closest_point_this_step = projected_closest
+        else:
+            closest_point_this_step = min(
+                (last_pos, current_pos),
+                key=lambda p: vec.distance_between(p, self._goal)
+            )
+        close = vec.distance_between(closest_point_this_step, self._goal) <= close_enough_tolerance
         stopped = self._mover.Velocity.Magnitude <= slow_enough_tolerance
 
+        if continue_after_goal_reached:
+            if close:
+                logger.info(f'Mover: {self.Name} passed through goal: {self._goal}')
+                self._mover.Acceleration.set(m_s2_vec=vec.zero_vector(len(vector_to_goal)))
+                return True
+
+            if decel_lookahead_distance is not None:
+                ''' Passing through this goal, but a real stop is coming up later along
+                the path: brake based on the TOTAL remaining distance to that eventual
+                stop (supplied by the caller), not the distance to this waypoint alone --
+                otherwise speed built up seeking through several pass-through waypoints
+                is "locked in" with no anticipation of the stop, and can arrive at the
+                final destination far too fast to avoid a large overshoot.
+
+                This is a single scalar accelerate/brake decision (speed vs. remaining
+                path distance), not a per-axis one: while accelerating, push toward this
+                waypoint same as plain seek; while braking, decelerate directly against
+                the current velocity vector. A per-axis version (independently deciding
+                each axis via arrival_acceleration) lets one axis start braking before the
+                other, since each reaches its own switching threshold at a different
+                time -- the combined force then no longer points along the direction of
+                travel, curving the mover off its path entirely. '''
+                speed = self.Velocity.Magnitude
+                scalar_accel = kin.scalar_lookahead_speed_accel(
+                    remaining_distance=decel_lookahead_distance,
+                    speed=speed,
+                    a=self.Acceleration.Max_Magnitude,
+                    dt=delta_time_ms / 1000.0,
+                )
+                accel_direction = vector_to_goal if scalar_accel >= 0 else self.Velocity.CurrentVelocity_M_S
+                self._mover.Acceleration.set(m_s2_vec=vec.scaled_to_length(accel_direction, scalar_accel))
+                return False
+
+            ''' Seek (not arrive): push at max acceleration straight toward the goal.
+            Unlike decide_how_to_accel_decel_towards_a_goal_without_overshoot, this never
+            brakes in anticipation of stopping here -- continue_after_goal_reached means
+            we don't intend to stop at this target, so there's nothing to avoid overshooting. '''
+            self._mover.Acceleration.set(m_s2_vec=vec.scaled_to_length(
+                vector_to_goal,
+                self.Acceleration.Max_Magnitude
+            ))
+            return False
+
+        ''' A tiny fixed epsilon, not close_enough_tolerance/slow_enough_tolerance --
+        this only guards against floating-point noise on an axis that's already
+        essentially exactly arrived. Using the caller's (potentially loose, realistic)
+        arrival tolerance here instead lets one axis independently freeze to zero
+        velocity well before the object has actually arrived as a whole, so a diagonal
+        approach looks like it abruptly stops on one axis while still sliding on the
+        other, rather than a single smooth, simultaneous decel-to-stop. '''
         for ii in range(len(self.Velocity.CurrentVelocity_M_S)):
-            if (self.Velocity.CurrentVelocity_M_S[ii] < slow_enough_tolerance and
-                    abs(vector_to_goal[ii]) < close_enough_tolerance):
+            if (self.Velocity.CurrentVelocity_M_S[ii] < 1e-6 and
+                    abs(vector_to_goal[ii]) < 1e-6):
                 self.Velocity.stop_in_idx(ii)
                 self.Acceleration.stop_in_idx(ii)
                 self.Transform.Translation.update(idx_val_map={ii: self._goal[ii]})
 
+        ''' The per-axis snap above can force-align every axis to the goal (each axis
+        individually within tolerance) even when the overall close/stopped check above
+        -- computed before the snap -- hadn't yet agreed (a diagonal case can satisfy
+        the looser per-axis criterion before the stricter combined-vector one). Without
+        refreshing here, the stale pre-snap vector_to_goal below computes a residual
+        acceleration that immediately nudges the mover away from the position it was
+        just snapped to, so "reached" can end up never firing. '''
+        current_pos = self._mover.Transform.Translation.Vector
+        vector_to_goal = vec.vector_between(current_pos, self._goal)
+        close = close or vec.distance_between(current_pos, self._goal) <= close_enough_tolerance
+        stopped = self._mover.Velocity.Magnitude <= slow_enough_tolerance
+
         if close and stopped:
             logger.info(f'Mover: {self.Name} has reached goal: {self._goal}')
             self._mover.update(
-                set_pos=goal_pos
+                set_pos=self._goal
             )
             self._mover.stop()
             return True
 
         ''' Update Velocity '''
-        self._mover.Acceleration.set(m_s2_vec=kin.decide_how_to_accel_decel_towards_a_goal_without_overshoot(
+        self._mover.Acceleration.set(m_s2_vec=kin.arrival_acceleration(
             vector_to_goal=vector_to_goal,
             velocity=self.Velocity.CurrentVelocity_M_S,
             accel_magnitude=self.Acceleration.Max_Magnitude,
+            dt=delta_time_ms / 1000.0,
         ))
 
         return False