diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index a016f1cec6..44bd3f72ca 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -14,6 +14,8 @@ MAX_DRAW_DISTANCE = 100.0 PATH_COLOR_TRANSITION_DURATION = 0.5 # Seconds for color transition animation PATH_BLEND_INCREMENT = 1.0 / (PATH_COLOR_TRANSITION_DURATION * DEFAULT_FPS) +MAX_POINTS = 200 + THROTTLE_COLORS = [ rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4) rl.Color(114, 255, 92, 89), # HSLF(112/360, 1.0, 0.68, 0.35) @@ -61,6 +63,11 @@ class ModelRenderer: self._transform_dirty = True self._clip_region = None self._rect = None + + # Pre-allocated arrays for polygon conversion + self._temp_points_3d = np.empty((MAX_POINTS * 2, 3), dtype=np.float32) + self._temp_proj = np.empty((3, MAX_POINTS * 2), dtype=np.float32) + self._exp_gradient = { 'start': (0.0, 1.0), # Bottom of path 'end': (0.0, 0.0), # Top of path @@ -140,10 +147,13 @@ class ModelRenderer: """Update positions of lead vehicles""" self._lead_vehicles = [LeadVehicle(), LeadVehicle()] leads = [radar_state.leadOne, radar_state.leadTwo] + for i, lead_data in enumerate(leads): if lead_data and lead_data.status: d_rel, y_rel, v_rel = lead_data.dRel, lead_data.yRel, lead_data.vRel idx = self._get_path_length_idx(path_x_array, d_rel) + + # Get z-coordinate from path at the lead vehicle position z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0 point = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z) if point: @@ -336,20 +346,22 @@ class ModelRenderer: return np.empty((0, 2), dtype=np.float32) # Slice points and filter non-negative x-coordinates - points = line[:max_idx + 1][line[:max_idx + 1, 0] >= 0] + points = line[:max_idx + 1] + points = points[points[:, 0] >= 0] if points.shape[0] == 0: return np.empty((0, 2), dtype=np.float32) # Create left and right 3D points in one array n_points = points.shape[0] - points_3d = np.empty((n_points * 2, 3), dtype=np.float32) + points_3d = self._temp_points_3d[:n_points * 2] points_3d[:n_points, 0] = points_3d[n_points:, 0] = points[:, 0] points_3d[:n_points, 1] = points[:, 1] - y_off points_3d[n_points:, 1] = points[:, 1] + y_off points_3d[:n_points, 2] = points_3d[n_points:, 2] = points[:, 2] + z_off # Single matrix multiplication for projections - proj = self._car_space_transform @ points_3d.T + proj = np.ascontiguousarray(self._temp_proj[:, :n_points * 2]) # Slice the pre-allocated array + np.dot(self._car_space_transform, points_3d.T, out=proj) valid_z = np.abs(proj[2]) > 1e-6 if not np.any(valid_z): return np.empty((0, 2), dtype=np.float32) @@ -390,15 +402,20 @@ class ModelRenderer: @staticmethod def _map_val(x, x0, x1, y0, y1): - x = max(x0, min(x, x1)) + x = np.clip(x, x0, x1) ra = x1 - x0 rb = y1 - y0 return (x - x0) * rb / ra + y0 if ra != 0 else y0 @staticmethod def _hsla_to_color(h, s, l, a): - r, g, b = [max(0, min(255, int(v * 255))) for v in colorsys.hls_to_rgb(h, l, s)] - return rl.Color(r, g, b, max(0, min(255, int(a * 255)))) + rgb = colorsys.hls_to_rgb(h, l, s) + return rl.Color( + int(rgb[0] * 255), + int(rgb[1] * 255), + int(rgb[2] * 255), + int(a * 255) + ) @staticmethod def _blend_colors(begin_colors, end_colors, t):