StarPilot/openpilot/selfdrive/ui/mici/onroad/model_renderer.py

import colorsys
import math
import numpy as np
import pyray as rl
from openpilot.cereal import log, messaging
from opendbc.car.structs import car
from dataclasses import dataclass, field
from types import SimpleNamespace
from openpilot.common.params import Params
from openpilot.starpilot.ui.appearance_preferences import format_lead_info
from openpilot.starpilot.ui.rainbow_path import RainbowPath
from openpilot.starpilot.ui.road_colors import lane_color, path_mode, solid_gradient
from openpilot.starpilot.ui.onroad_customization import ROAD_COLORS
from openpilot.starpilot.lateral.lane_feedback import BLUE, direction as lane_centering_direction
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.selfdrive.ui.mici.onroad import blend_colors
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.application import FontWeight
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
from openpilot.system.ui.widgets import Widget

CLIP_MARGIN = 500
MIN_DRAW_DISTANCE = 10.0
MAX_DRAW_DISTANCE = 100.0
LEAD_BAR_LENGTH = 12.0
LEAD_BAR_WIDTH = 1.8

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)
  rl.Color(114, 255, 92, 0),     # HSLF(112/360, 1.0, 0.68, 0.0)
]

NO_THROTTLE_COLORS = [
  rl.Color(242, 242, 242, 102), # HSLF(148/360, 0.0, 0.95, 0.4)
  rl.Color(242, 242, 242, 89),  # HSLF(112/360, 0.0, 0.95, 0.35)
  rl.Color(242, 242, 242, 0),   # HSLF(112/360, 0.0, 0.95, 0.0)
]

LANE_LINE_COLORS = {
  UIStatus.DISENGAGED: rl.Color(200, 200, 200, 255),
  UIStatus.OVERRIDE: rl.Color(255, 255, 255, 255),
  UIStatus.ENGAGED: rl.Color(0, 255, 64, 255),
}


@dataclass
class ModelPoints:
  raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32))
  projected_points: np.ndarray = field(default_factory=lambda: np.empty((0, 2), dtype=np.float32))


class LeadVehicle:
  def __init__(self):
    self.info = None
    self.bar = np.empty((0, 2), dtype=np.float32)
    self.d_filter = FirstOrderFilter(0.0, 0.2, 1 / gui_app.target_fps, initialized=False)
    self.y_filter = FirstOrderFilter(0.0, 0.2, 1 / gui_app.target_fps, initialized=False)
    self.fade_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)




class ModelRenderer(Widget):
  def __init__(self):
    super().__init__()
    self._longitudinal_control = False
    self._experimental_mode = False
    self._blend_filter = FirstOrderFilter(1.0, 0.25, 1 / gui_app.target_fps)
    self._prev_allow_throttle = True
    self._default_path_gradient = None
    self._default_path_blend_factor = None
    self._lane_line_probs = np.zeros(4, dtype=np.float32)
    self._lane_centering_direction = 0
    self._road_edge_stds = np.zeros(2, dtype=np.float32)
    self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
    self._leads_need_reset = False
    self._lead_indicator_enabled = False
    self._lead_info_mode = 0
    self._lead_info_metric = None
    self._render_lateral_active = False
    self._path_offset_z = HEIGHT_INIT[0]

    # Initialize ModelPoints objects
    self._path = ModelPoints()
    self._lane_lines = [ModelPoints() for _ in range(4)]
    self._road_edges = [ModelPoints() for _ in range(2)]
    self._acceleration_x = np.empty((0,), dtype=np.float32)

    self._acceleration_x_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
    self._acceleration_x_filter2 = FirstOrderFilter(0.0, 1, 1 / gui_app.target_fps)

    self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
    self._ll_color_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)

    # 3x3 car space -> rect-origin space (draw methods add rect.x/y)
    self._car_space_transform = np.zeros((3, 3), dtype=np.float32)
    self._transform_dirty = True
    self._clip_region = None
    self._lane_projection_clip = None

    self._exp_gradient = Gradient(
      start=(0.0, 1.0),  # Bottom of path
      end=(0.0, 0.0),  # Top of path
      colors=[],
      stops=[],
    )
    self._rainbow_path = RainbowPath()
    self.road_style = {}

    # Get longitudinal control setting from car parameters
    if car_params := Params().get("CarParams"):
      cp = messaging.log_from_bytes(car_params, car.CarParams)
      self._longitudinal_control = cp.openpilotLongitudinalControl

  def set_transform(self, transform: np.ndarray):
    transform = transform.astype(np.float32)
    if not np.array_equal(transform, self._car_space_transform):
      self._car_space_transform = transform
      self._transform_dirty = True

  def render_with_lead(self, rect: rl.Rectangle, enabled: bool, info_mode: int = 0, metric: bool | None = None,
                       lateral_active: bool = False, paint: bool = True):
    self._lead_indicator_enabled = enabled
    self._lead_info_mode = info_mode
    self._lead_info_metric = metric
    self._render_lateral_active = lateral_active
    self._paint = paint
    try:
      return self.render(rect)
    finally:
      self._lead_indicator_enabled = False
      self._lead_info_mode = 0
      self._lead_info_metric = None
      self._render_lateral_active = False
      self._paint = True

  def _visual_status(self) -> UIStatus:
    return UIStatus.ENGAGED if ui_state.status == UIStatus.DISENGAGED and getattr(self, '_render_lateral_active', False) else ui_state.status

  def _render(self, rect: rl.Rectangle):
    sm = ui_state.sm
    replay = getattr(ui_state, 'replay_clock', None) is not None
    if replay and any(not sm.seen[service] or not sm.valid[service] or not sm.alive[service]
                      for service in ('modelV2', 'extrinsicsCalibration')):
      return

    self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque)

    # Check if data is up-to-date
    if (sm.recv_frame["extrinsicsCalibration"] < ui_state.started_frame or
        sm.recv_frame["modelV2"] < ui_state.started_frame):
      return

    # Set up clipping region
    self._clip_region = rl.Rectangle(
      rect.x - CLIP_MARGIN, rect.y - CLIP_MARGIN, rect.width + 2 * CLIP_MARGIN, rect.height + 2 * CLIP_MARGIN
    )

    # Update state
    self._experimental_mode = sm['selfdriveState'].experimentalMode

    extrinsics_calibration = sm['extrinsicsCalibration']
    self._path_offset_z = extrinsics_calibration.height[0] if extrinsics_calibration.height else HEIGHT_INIT[0]

    if sm.updated['carParams']:
      self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl

    model = sm['modelV2']
    radar_state = sm['radarState'] if sm.valid['radarState'] else None
    lead_one = radar_state.leadOne if radar_state else None
    if lead_one and lead_one.present and not self._valid_lead_numbers(lead_one):
      lead_one = None
    render_lead_indicator = self._lead_indicator_enabled

    # Update model data when needed
    model_updated = sm.updated['modelV2']
    if model_updated or sm.updated['radarState'] or self._transform_dirty:
      if model_updated:
        self._update_raw_points(model)

      path_x_array = self._path.raw_points[:, 0]
      if path_x_array.size == 0:
        return

      self._update_model(lead_one, path_x_array, update_lane_geometry=model_updated or self._transform_dirty)
      self._transform_dirty = False

    lead_ready = render_lead_indicator and bool(self._path.raw_points.size)
    if lead_ready:
      self._update_leads(sm)
      self._leads_need_reset = True
    elif getattr(self, "_leads_need_reset", True):
      self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
      self._leads_need_reset = False

    # Draw elements (hide when disengaged)
    if self._visual_status() != UIStatus.DISENGAGED:
      self._draw_lane_lines()
      self._draw_path(sm)

    if lead_ready and getattr(self, '_paint', True):
      self._draw_lead_indicator()
      if self._lead_vehicles[0].bar.size and self._lead_vehicles[0].info is not None:
        text = self._format_lead_info(self._lead_vehicles[0].info, self._lead_info_mode, self._lead_info_metric)
        if text:
          self._draw_lead_info(rect, text)

  @staticmethod
  def _draw_lead_info(rect: rl.Rectangle, text: str) -> None:
    font = gui_app.font(FontWeight.SEMI_BOLD)
    size = measure_text_cached(font, text, 40)
    x, y = rect.x + (rect.width - size.x) / 2, rect.y + 22
    for dx, dy in ((-1, -1), (1, -1), (-1, 1), (1, 1)):
      rl.draw_text_ex(font, text, rl.Vector2(x + dx, y + dy), 40, 0, rl.BLACK)
    rl.draw_text_ex(font, text, rl.Vector2(x, y), 40, 0, rl.WHITE)

  @staticmethod
  def _format_lead_info(lead, mode: int, metric: bool | None) -> str:
    return format_lead_info(lead, mode, metric)

  @staticmethod
  def _valid_lead_numbers(lead) -> bool:
    try:
      distance, lateral, relative_speed = float(lead.dRel), float(lead.yRel), float(lead.vRel)
      return distance > 0 and all(math.isfinite(value) for value in (distance, lateral, relative_speed))
    except (AttributeError, TypeError, ValueError, OverflowError):
      return False

  def _update_raw_points(self, model):
    """Update raw 3D points from model data"""
    self._path.raw_points = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T

    # Older logs may append lane centers after the four boundaries.
    for points, lane_line in zip(self._lane_lines, model.laneLines, strict=False):
      points.raw_points = np.array([lane_line.x, lane_line.y, lane_line.z], dtype=np.float32).T

    for i, road_edge in enumerate(model.roadEdges):
      self._road_edges[i].raw_points = np.array([road_edge.x, road_edge.y, road_edge.z], dtype=np.float32).T

    self._lane_line_probs = np.array(model.laneLineProbs, dtype=np.float32)
    self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32)
    self._acceleration_x = np.array(model.acceleration.x, dtype=np.float32)

  def _update_leads(self, sm):
    plan = sm['longitudinalPlan']
    model = sm['modelV2']
    e2e = (sm.valid['longitudinalPlan'] and sm.valid['modelV2'] and
           plan.longitudinalPlanSource == log.LongitudinalPlan.LongitudinalPlanSource.e2e and len(model.leadsV3) > 1)
    leads = []
    for index in range(2):
      info = None
      if e2e:
        candidate = model.leadsV3[index]
        if candidate.x and candidate.y and candidate.v:
          info = SimpleNamespace(present=candidate.prob > 0.5, dRel=candidate.x[0] - RADAR_TO_CAMERA,
                                 yRel=-candidate.y[0], vRel=0.0, vLead=candidate.v[0])
      elif sm.valid['radarState']:
        info = (sm['radarState'].leadOne, sm['radarState'].leadTwo)[index]
      valid = info is not None and info.present and self._valid_lead_numbers(info)
      distance = float(info.dRel) + RADAR_TO_CAMERA if valid else 0.0
      leads.append((valid, distance, float(info.yRel) if valid else 0.0, info if valid else None))
    if leads[0][0] and leads[1][0] and abs(leads[1][1] - leads[0][1]) < 3.0:
      leads[1] = (False, 0.0, 0.0, None)

    ss, cs = sm['selfdriveState'], sm['carState']
    available = ss.enabled or ss.engageable or cs.brakePressed or getattr(self, '_render_lateral_active', False)
    lane = self._path.raw_points
    geometry_valid = len(lane) > 0 and len(self._path.raw_points) > 0 and np.isfinite(lane).all() and np.isfinite(self._path.raw_points).all()
    opacity = 0.4 if self._visual_status() == UIStatus.DISENGAGED else 0.8
    for lead, (present, d_rel, y_rel, info) in zip(self._lead_vehicles, leads, strict=True):
      visible = available and present and d_rel < MAX_DRAW_DISTANCE and geometry_valid
      if not visible or abs(y_rel - lead.y_filter.x) > 3.0:
        lead.d_filter.initialized = lead.y_filter.initialized = False
      lead.fade_filter.update(opacity if visible else 0.0)
      lead.info = info if visible else None
      if visible:
        lead.bar = self._get_lead_bar(lane, lead.d_filter.update(d_rel), lead.y_filter.update(y_rel))
        if not lead.bar.size:
          lead.info = None
          lead.fade_filter.update(0.0)
      elif not present or not geometry_valid:
        lead.bar = np.empty((0, 2), dtype=np.float32)


  def _get_lead_bar(self, lane, d_rel, y_rel):
    # bar on the road behind the lead, following the lane
    x = np.array([d_rel, d_rel - min(6.0, 0.25 * d_rel)])
    y = np.interp(x, lane[:, 0], lane[:, 1]) - np.interp(d_rel, lane[:, 0], lane[:, 1]) - y_rel
    z = np.interp(x, self._path.raw_points[:, 0], self._path.raw_points[:, 2]) + self._path_offset_z
    corners = np.vstack((np.column_stack((x, y + LEAD_BAR_WIDTH / 2, z)), np.column_stack((x, y - LEAD_BAR_WIDTH / 2, z))[::-1]))
    pts = self._car_space_transform @ corners.T
    if not np.isfinite(pts).all() or np.any(np.abs(pts[2]) < 1e-6):
      return np.empty((0, 2), dtype=np.float32)
    bar = (pts[:2] / pts[2]).T

    far, near = bar[[0, 3]], bar[[1, 2]]
    length = np.linalg.norm(near.mean(axis=0) - far.mean(axis=0))
    if not math.isfinite(length) or length < 1e-6:
      return np.empty((0, 2), dtype=np.float32)
    bar[[1, 2]] = far + (near - far) * np.clip(length, 3.0, LEAD_BAR_LENGTH) / length
    return bar.astype(np.float32)



  def _update_model(self, lead, path_x_array, *, update_lane_geometry: bool = True):
    """Update model visualization data based on model message"""
    max_distance = np.clip(path_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE)
    clip = self._clip_region
    clip_key = (clip.x, clip.y, clip.width, clip.height)
    if update_lane_geometry or clip_key != getattr(self, "_lane_projection_clip", None):
      max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance)

      # Radar changes the path's lead-distance clipping, not lane/edge geometry.
      lines = [*self._lane_lines, *self._road_edges]
      widths = [width * probability for width, probability in
                zip((0.12, 0.16, 0.16, 0.16), self._lane_line_probs, strict=True)]
      widths.extend((0.16, 0.16))
      polygons = self._map_lines_to_polygons([line.raw_points for line in lines], widths, max_idx)
      for line, polygon in zip(lines, polygons, strict=True):
        line.projected_points = polygon
      self._lane_projection_clip = clip_key

    # Update path using raw points
    if lead and lead.present:
      lead_d = lead.dRel * 2.0
      max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance)

    soon_acceleration = self._acceleration_x[len(self._acceleration_x) // 4] if len(self._acceleration_x) > 0 else 0
    self._acceleration_x_filter.update(soon_acceleration)
    self._acceleration_x_filter2.update(soon_acceleration)

    # make path width wider/thinner when initially braking/accelerating
    if self._experimental_mode and False:
      high_pass_acceleration = self._acceleration_x_filter.x - self._acceleration_x_filter2.x
      y_off = np.interp(high_pass_acceleration, [-1, 0, 1], [0.9 * 2, 0.9, 0.9 / 2])
    else:
      y_off = 0.9

    max_idx = self._get_path_length_idx(path_x_array, max_distance)
    self._path.projected_points = self._map_line_to_polygon(
      self._path.raw_points, y_off, self._path_offset_z, max_idx, allow_invert=False
    )

    self._update_experimental_gradient()

  def _update_experimental_gradient(self):
    """Pre-calculate experimental mode gradient colors"""
    if not self._experimental_mode:
      return

    path_pts = self._path.projected_points + np.array([self._rect.x, self._rect.y], dtype=np.float32)
    max_len = min(len(path_pts) // 2, len(self._acceleration_x))

    segment_colors = []
    gradient_stops = []

    i = 0
    while i < max_len:
      # Some points (screen space) are out of frame (rect space)
      track_y = path_pts[i][1]
      if track_y < self._rect.y or track_y > (self._rect.y + self._rect.height):
        i += 1
        continue

      # Calculate color based on acceleration (0 is bottom, 1 is top)
      lin_grad_point = 1 - (track_y - self._rect.y) / self._rect.height

      # speed up: 120, slow down: 0
      path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120)

      saturation = min(abs(self._acceleration_x[i] * 1.5), 1)
      lightness = np.interp(saturation, [0.0, 1.0], [0.95, 0.62])
      alpha = np.interp(lin_grad_point, [0.75 / 2.0, 0.75], [0.4, 0.0])

      # Use HSL to RGB conversion
      color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha)

      gradient_stops.append(lin_grad_point)
      segment_colors.append(color)

      # Skip a point, unless next is last
      i += 1 + (1 if (i + 2) < max_len else 0)

    # Store the gradient in the path object
    self._exp_gradient.colors = segment_colors
    self._exp_gradient.stops = gradient_stops



  def _get_ll_color(self, prob: float, adjacent: bool, left: bool):
    alpha = 0.0 if prob < 0.0 else 0.7 if prob > 0.7 else prob
    if adjacent:
      _base_color = LANE_LINE_COLORS.get(self._visual_status(), LANE_LINE_COLORS[UIStatus.DISENGAGED])
      color = rl.Color(_base_color.r, _base_color.g, _base_color.b, int(alpha * 255))
      color = lane_color(getattr(self, "road_style", {}), True, color, prob)

      # turn adjacent lls orange if torque is high
      torque = self._torque_filter.x
      high_torque = abs(torque) > 0.6
      if high_torque and (left == (torque > 0)):
        color = blend_colors(
          color,
          rl.Color(255, 115, 0, int(alpha * 255)),  # orange
          np.interp(abs(torque), [0.6, 0.8], [0.0, 1.0])
        )
    else:
      color = rl.Color(255, 255, 255, int(alpha * 255))
      color = lane_color(getattr(self, "road_style", {}), False, color, prob)

    if self._visual_status() == UIStatus.DISENGAGED:
      color = rl.Color(0, 0, 0, int(alpha * 255))

    return color

  def _draw_lane_lines(self):
    """Draw lane lines and road edges. Two closest lines should be green (lane line or road edges)."""
    offset = np.array([self._rect.x, self._rect.y], dtype=np.float32)
    self._lane_centering_direction = lane_centering_direction(ui_state.sm, ui_state.display_time_ns(), ui_state.started_frame)
    if not getattr(self, '_paint', True):
      return

    for i, lane_line in enumerate(self._lane_lines):
      if lane_line.projected_points.size == 0:
        continue

      color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1))
      if (i == 1 and self._lane_centering_direction < 0) or (i == 2 and self._lane_centering_direction > 0):
        color = rl.Color(*BLUE, int(np.clip(self._lane_line_probs[i], 0.0, 0.7) * 255))
      draw_polygon(self._rect, lane_line.projected_points + offset, color)

    for i, road_edge in enumerate(self._road_edges):
      if road_edge.projected_points.size == 0:
        continue

      # if closest lane lines are not confident, make road edges green
      color = self._get_ll_color(float(1.0 - self._road_edge_stds[i]), float(self._lane_line_probs[i + 1]) < 0.25, i == 0)
      draw_polygon(self._rect, road_edge.projected_points + offset, color)

  def _draw_path(self, sm):
    """Draw path with dynamic coloring based on mode and throttle state."""
    if not self._path.projected_points.size:
      return

    replay = getattr(ui_state, 'replay_clock', None) is not None
    plan_known = not replay or (sm.seen['longitudinalPlan'] and sm.valid['longitudinalPlan'] and sm.alive['longitudinalPlan'])
    if plan_known:
      allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control
      self._blend_filter.update(int(allow_throttle))

    now_ns = ui_state.display_time_ns()
    style = getattr(self, "road_style", {})
    mode = path_mode(style)
    if mode == "rainbow":
      car_fresh = bool(sm.valid.get('carState', False) and sm.alive.get('carState', False) and
                       sm.recv_frame['carState'] >= ui_state.started_frame)
      speed = float(sm['carState'].vEgo) if car_fresh and sm.updated['carState'] else None
      self._rainbow_path.update(now_ns, speed, source_alive=car_fresh)
    if not getattr(self, '_paint', True):
      return

    path_pts = self._path.projected_points + np.array([self._rect.x, self._rect.y], dtype=np.float32)
    if mode == "rainbow":
      draw_polygon(self._rect, path_pts, gradient=self._rainbow_path.gradient())
      return
    if mode == "color":
      draw_polygon(self._rect, path_pts, gradient=solid_gradient(style.get("path", ROAD_COLORS["path"])))
      return

    if not self._experimental_mode and not plan_known:
      draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30))
      return

    if self._experimental_mode:
      # Draw with acceleration coloring
      if self._visual_status() == UIStatus.DISENGAGED:
        draw_polygon(self._rect, path_pts, rl.Color(0, 0, 0, 90))
      elif len(self._exp_gradient.colors) > 1:
        draw_polygon(self._rect, path_pts, gradient=self._exp_gradient)
      else:
        draw_polygon(self._rect, path_pts, rl.Color(255, 255, 255, 30))
    else:
      # Blend throttle/no throttle colors based on transition
      blend_factor = round(self._blend_filter.x * 100) / 100
      if blend_factor != getattr(self, "_default_path_blend_factor", None):
        self._default_path_gradient = Gradient(
          start=(0.0, 1.0),  # Bottom of path
          end=(0.0, 0.0),  # Top of path
          colors=self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor),
          stops=[0.0, 0.5, 1.0],
        )
        self._default_path_blend_factor = blend_factor
      gradient = self._default_path_gradient

      if self._visual_status() == UIStatus.DISENGAGED:
        draw_polygon(self._rect, path_pts, rl.Color(0, 0, 0, 90))
      else:
        draw_polygon(self._rect, path_pts, gradient=gradient)

  def _draw_lead_indicator(self):
    offset = np.array([self._rect.x, self._rect.y], dtype=np.float32)
    for lead in self._lead_vehicles:
      if not lead.bar.size or lead.fade_filter.x <= 0:
        continue
      draw_polygon(self._rect, lead.bar + offset, rl.Color(255, 255, 255, int(255 * lead.fade_filter.x)))



  @staticmethod
  def _get_path_length_idx(pos_x_array: np.ndarray, path_height: 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



  def _map_line_to_polygon(self, line: np.ndarray, y_off: float, z_off: float, max_idx: int, allow_invert: bool = True) -> np.ndarray:
    """Convert 3D line to 2D polygon for rendering."""
    if line.shape[0] == 0:
      return np.empty((0, 2), dtype=np.float32)

    # Slice points and filter non-negative x-coordinates
    points = line[:max_idx + 1]
    points = points[points[:, 0] >= 0]
    if points.shape[0] == 0:
      return np.empty((0, 2), dtype=np.float32)

    N = points.shape[0]
    # Generate left and right 3D points in one array using broadcasting
    offsets = np.array([[0, -y_off, z_off], [0, y_off, z_off]], dtype=np.float32)
    points_3d = points[None, :, :] + offsets[:, None, :]  # Shape: 2xNx3
    points_3d = points_3d.reshape(2 * N, 3)  # Shape: (2*N)x3

    # Transform all points to projected space in one operation
    proj = self._car_space_transform @ points_3d.T  # Shape: 3x(2*N)
    proj = proj.reshape(3, 2, N)
    left_proj = proj[:, 0, :]
    right_proj = proj[:, 1, :]

    # Filter points where z is sufficiently large
    valid_proj = (np.abs(left_proj[2]) >= 1e-6) & (np.abs(right_proj[2]) >= 1e-6)
    if not np.any(valid_proj):
      return np.empty((0, 2), dtype=np.float32)

    # Compute screen coordinates
    left_screen = left_proj[:2, valid_proj] / left_proj[2, valid_proj][None, :]
    right_screen = right_proj[:2, valid_proj] / right_proj[2, valid_proj][None, :]

    # Define clip region bounds
    clip = self._clip_region
    x_min, x_max = clip.x, clip.x + clip.width
    y_min, y_max = clip.y, clip.y + clip.height

    # Filter points within clip region
    left_in_clip = (
      (left_screen[0] >= x_min) & (left_screen[0] <= x_max) &
      (left_screen[1] >= y_min) & (left_screen[1] <= y_max)
    )
    right_in_clip = (
      (right_screen[0] >= x_min) & (right_screen[0] <= x_max) &
      (right_screen[1] >= y_min) & (right_screen[1] <= y_max)
    )
    both_in_clip = left_in_clip & right_in_clip

    if not np.any(both_in_clip):
      return np.empty((0, 2), dtype=np.float32)

    # Select valid and clipped points
    left_screen = left_screen[:, both_in_clip]
    right_screen = right_screen[:, both_in_clip]

    # Handle Y-coordinate inversion on hills
    if not allow_invert and left_screen.shape[1] > 1:
      y = left_screen[1, :]  # y-coordinates
      keep = y == np.minimum.accumulate(y)
      if not np.any(keep):
        return np.empty((0, 2), dtype=np.float32)
      left_screen = left_screen[:, keep]
      right_screen = right_screen[:, keep]

    return np.vstack((left_screen.T, right_screen[:, ::-1].T)).astype(np.float32)

  def _map_lines_to_polygons(self, lines: list[np.ndarray], widths: list[float], max_idx: int) -> list[np.ndarray]:
    """Project clipped lane/edge pairs together while preserving per-line masks."""
    points = [line[:max_idx + 1][line[:max_idx + 1, 0] >= 0] for line in lines]
    counts = [len(line) for line in points]
    total = sum(counts)
    if total == 0:
      return [np.empty((0, 2), dtype=np.float32) for _ in lines]
    offsets = np.zeros((2, total, 3), dtype=np.float32)
    offsets[1, :, 1] = np.repeat(widths, counts)
    offsets[0, :, 1] = -offsets[1, :, 1]
    joined = np.concatenate(points)
    proj = (self._car_space_transform @ (joined[None, :, :] + offsets).reshape(2 * total, 3).T).reshape(3, 2, total)
    valid = (np.abs(proj[2, 0]) >= 1e-06) & (np.abs(proj[2, 1]) >= 1e-06)
    # Divide and clip all projected pairs together; retain each line's output order.
    left = np.zeros((2, total), dtype=proj.dtype)
    right = np.zeros((2, total), dtype=proj.dtype)
    np.divide(proj[:2, 0], proj[2, 0][None, :], out=left, where=valid[None, :])
    np.divide(proj[:2, 1], proj[2, 1][None, :], out=right, where=valid[None, :])
    clip = self._clip_region
    keep = valid & ((left[0] >= clip.x) & (left[0] <= clip.x + clip.width) &
            (left[1] >= clip.y) & (left[1] <= clip.y + clip.height) &
            (right[0] >= clip.x) & (right[0] <= clip.x + clip.width) &
            (right[1] >= clip.y) & (right[1] <= clip.y + clip.height))
    polygons = []
    start = 0
    for count in counts:
      end = start + count
      l = left[:, start:end][:, keep[start:end]]
      r = right[:, start:end][:, keep[start:end]]
      polygons.append(np.vstack((l.T, r[:, ::-1].T)).astype(np.float32) if l.shape[1] else np.empty((0, 2), dtype=np.float32))
      start = end
    return polygons

  @staticmethod
  def _hsla_to_color(h, s, l, a):
    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):
    if t >= 1.0:
      return end_colors
    if t <= 0.0:
      return begin_colors

    inv_t = 1.0 - t
    return [rl.Color(
      int(inv_t * start.r + t * end.r),
      int(inv_t * start.g + t * end.g),
      int(inv_t * start.b + t * end.b),
      int(inv_t * start.a + t * end.a)
    ) for start, end in zip(begin_colors, end_colors, strict=True)]