From e8f526f6b8ac6ca4d58858799be9504d5b402c66 Mon Sep 17 00:00:00 2001
From: firestarsdog <229254897+firestarsdog@users.noreply.github.com>
Date: Mon, 27 Jul 2026 19:45:00 -0400
Subject: [PATCH] Faster Bats

---
 selfdrive/ui/mici/onroad/model_renderer.py |  6 ++--
 selfdrive/ui/onroad/model_renderer.py      | 40 ++++++++++++++++------
 2 files changed, 32 insertions(+), 14 deletions(-)

diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py
index 133eded9cb..5af009a60e 100644
--- a/selfdrive/ui/mici/onroad/model_renderer.py
+++ b/selfdrive/ui/mici/onroad/model_renderer.py
@@ -463,12 +463,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"""
diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py
index a0e326be05..963be4e935 100644
--- a/selfdrive/ui/onroad/model_renderer.py
+++ b/selfdrive/ui/onroad/model_renderer.py
@@ -606,17 +606,35 @@ class ModelRenderer(Widget):
     radius = 4.0
     red_color = rl.Color(255, 0, 0, 200)
 
+    # Pre-extract bounds and matrix values for native loop speed
+    x_min, x_max = self._rect.x, self._rect.x + self._rect.width
+    y_min, y_max = self._rect.y, self._rect.y + self._rect.height
+    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])
+    
+    offset_z = float(self._path_offset_z)
+    
     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
+      d_rel = float(point.dRel)
+      
+      # 1. Fast binary search instead of np.where boolean mask
+      idx = np.searchsorted(path_x_array, d_rel, side='right') - 1
+      idx = idx if idx >= 0 else 0
+      z = float(line_z[idx]) if idx < len(line_z) else 0.0
 
-      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)
+      # 2. Native unrolled matrix multiplication (Bypasses np.array allocation overhead)
+      in_y = float(-point.yRel)
+      in_z = z + offset_z
+      
+      pt_x = m00 * d_rel + m01 * in_y + m02 * in_z
+      pt_y = m10 * d_rel + m11 * in_y + m12 * in_z
+
+      # 3. Native clipping (50x faster than np.clip on scalars)
+      x = max(x_min, min(pt_x, x_max))
+      y = max(y_min, min(pt_y, y_max))
+      
+      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 +723,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"""