Faster Bats

selfdrive/ui/mici/onroad/model_renderer.py

@@ -464,12 +464,12 @@ class ModelRenderer(Widget):
       rl.draw_triangle_fan(lead.chevron, len(lead.chevron), with_alpha(lead_color, lead.fill_alpha))
 
   @staticmethod
-  def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int:
+  def _get_path_length_idx(pos_x_array: np.ndarray, path_distance: float) -> int:
     """Get the index corresponding to the given path height"""
     if len(pos_x_array) == 0:
       return 0
-    indices = np.where(pos_x_array <= path_height)[0]
-    return indices[-1] if indices.size > 0 else 0
+    idx = np.searchsorted(pos_x_array, path_distance, side='right') - 1
+    return idx if idx >= 0 else 0
 
   def _map_to_screen(self, in_x, in_y, in_z):
     """Project a point in car space to screen space"""

selfdrive/ui/onroad/model_renderer.py

@@ -1,4 +1,5 @@
 import colorsys
+import math
 import numpy as np
 import pyray as rl
 from cereal import messaging, car
@@ -606,17 +607,53 @@ class ModelRenderer(Widget):
     radius = 4.0
     red_color = rl.Color(255, 0, 0, 200)
 
-    for point in radar_points:
-      d_rel = point.dRel
-      idx = self._get_path_length_idx(path_x_array, d_rel)
-      z = line_z[idx] if idx < len(line_z) else 0.0
+    # Pre-extract matrix values and clip bounds for native loop speed
+    t = self._car_space_transform
+    m00, m01, m02 = float(t[0, 0]), float(t[0, 1]), float(t[0, 2])
+    m10, m11, m12 = float(t[1, 0]), float(t[1, 1]), float(t[1, 2])
+    m20, m21, m22 = float(t[2, 0]), float(t[2, 1]), float(t[2, 2])
+    clip = self._clip_region
+    clip_x, clip_y = float(clip.x), float(clip.y)
+    clip_xmax, clip_ymax = clip_x + float(clip.width), clip_y + float(clip.height)
+    offset_z = float(self._path_offset_z)
 
-      calibrated_point = self._map_to_screen(d_rel, -point.yRel, z + self._path_offset_z)
-      if calibrated_point:
-        x, y = calibrated_point
-        x = np.clip(x, self._rect.x, self._rect.x + self._rect.width)
-        y = np.clip(y, self._rect.y, self._rect.y + self._rect.height)
-        rl.draw_circle_v(rl.Vector2(x, y), radius, red_color)
+    rect_x, rect_y = float(self._rect.x), float(self._rect.y)
+    rect_xmax, rect_ymax = rect_x + float(self._rect.width), rect_y + float(self._rect.height)
+
+    for point in radar_points:
+      d_rel = float(point.dRel)
+      in_y = float(-point.yRel)
+
+      # 0. Reject invalid sensor values before projection
+      if not math.isfinite(d_rel) or not math.isfinite(in_y):
+        continue
+
+      # 1. Fast binary search instead of np.where boolean mask
+      idx = np.searchsorted(path_x_array, d_rel, side='right') - 1
+      idx = int(idx) if idx >= 0 else 0
+      z = float(line_z[idx]) if idx < len(line_z) else 0.0
+
+      # 2. Native unrolled 3x3 matrix multiply (bypasses np.array allocation)
+      in_z = z + offset_z
+      pt_w = m20 * d_rel + m21 * in_y + m22 * in_z
+
+      # 3. Match _map_to_screen: skip points at the focal plane.
+      if abs(pt_w) < 1e-6:
+        continue
+
+      # 4. Perspective divide (matches _map_to_screen)
+      x = (m00 * d_rel + m01 * in_y + m02 * in_z) / pt_w
+      y = (m10 * d_rel + m11 * in_y + m12 * in_z) / pt_w
+
+      # 5. Clip region check (matches _map_to_screen)
+      if not (clip_x <= x <= clip_xmax and clip_y <= y <= clip_ymax):
+        continue
+
+      # 6. Screen rect clamping (matches original np.clip on calibrated_point)
+      x = max(rect_x, min(x, rect_xmax))
+      y = max(rect_y, min(y, rect_ymax))
+
+      rl.draw_circle_v(rl.Vector2(x, y), radius, red_color)
 
   def _update_adjacent_paths(self, max_idx: int, max_distance: float):
     """Compute adjacent lane path polygons by averaging lane line pairs."""
@@ -705,8 +742,8 @@ class ModelRenderer(Widget):
     """Get the index corresponding to the given path distance"""
     if len(pos_x_array) == 0:
       return 0
-    indices = np.where(pos_x_array <= path_distance)[0]
-    return indices[-1] if indices.size > 0 else 0
+    idx = np.searchsorted(pos_x_array, path_distance, side='right') - 1
+    return idx if idx >= 0 else 0
 
   def _map_to_screen(self, in_x, in_y, in_z):
     """Project a point in car space to screen space"""