diff --git a/system/ui/lib/shader_polygon.py b/system/ui/lib/shader_polygon.py new file mode 100644 index 0000000000..dfbfa969d5 --- /dev/null +++ b/system/ui/lib/shader_polygon.py @@ -0,0 +1,338 @@ +import pyray as rl +import numpy as np +from typing import Any + + +FRAGMENT_SHADER = """ +#version 300 es +precision mediump float; + +in vec2 fragTexCoord; +out vec4 finalColor; + +uniform vec2 points[100]; +uniform int pointCount; +uniform vec4 fillColor; +uniform vec2 resolution; + +uniform bool useGradient; +uniform vec2 gradientStart; +uniform vec2 gradientEnd; +uniform vec4 gradientColors[8]; +uniform float gradientStops[8]; +uniform int gradientColorCount; + +vec4 getGradientColor(vec2 pos) { + vec2 gradientDir = gradientEnd - gradientStart; + float gradientLength = length(gradientDir); + + if (gradientLength < 0.001) return gradientColors[0]; + + vec2 normalizedDir = gradientDir / gradientLength; + vec2 pointVec = pos - gradientStart; + float projection = dot(pointVec, normalizedDir); + float t = clamp(projection / gradientLength, 0.0, 1.0); + + for (int i = 0; i < gradientColorCount - 1; i++) { + if (t >= gradientStops[i] && t <= gradientStops[i+1]) { + float segmentT = (t - gradientStops[i]) / (gradientStops[i+1] - gradientStops[i]); + return mix(gradientColors[i], gradientColors[i+1], segmentT); + } + } + + return gradientColors[gradientColorCount-1]; +} + +bool isPointInsidePolygon(vec2 p) { + if (pointCount < 3) return false; + + if (pointCount == 3) { + vec2 v0 = points[0]; + vec2 v1 = points[1]; + vec2 v2 = points[2]; + + float d = (v1.y - v2.y) * (v0.x - v2.x) + (v2.x - v1.x) * (v0.y - v2.y); + if (abs(d) < 0.0001) return false; + + float a = ((v1.y - v2.y) * (p.x - v2.x) + (v2.x - v1.x) * (p.y - v2.y)) / d; + float b = ((v2.y - v0.y) * (p.x - v2.x) + (v0.x - v2.x) * (p.y - v2.y)) / d; + float c = 1.0 - a - b; + + return (a >= 0.0 && b >= 0.0 && c >= 0.0); + } + + bool inside = false; + for (int i = 0, j = pointCount - 1; i < pointCount; j = i++) { + if (distance(points[i], points[j]) < 0.0001) continue; + + float dy = points[j].y - points[i].y; + if (abs(dy) < 0.0001) continue; + + if (((points[i].y > p.y) != (points[j].y > p.y))) { + float x_intersect = points[i].x + (points[j].x - points[i].x) * (p.y - points[i].y) / dy; + if (p.x < x_intersect) { + inside = !inside; + } + } + } + return inside; +} + +float distanceToEdge(vec2 p) { + float minDist = 1000.0; + + for (int i = 0, j = pointCount - 1; i < pointCount; j = i++) { + vec2 edge0 = points[j]; + vec2 edge1 = points[i]; + + if (distance(edge0, edge1) < 0.0001) continue; + + vec2 v1 = p - edge0; + vec2 v2 = edge1 - edge0; + float l2 = dot(v2, v2); + + if (l2 < 0.0001) { + float dist = length(v1); + minDist = min(minDist, dist); + continue; + } + + float t = clamp(dot(v1, v2) / l2, 0.0, 1.0); + vec2 projection = edge0 + t * v2; + float dist = length(p - projection); + minDist = min(minDist, dist); + } + + return minDist; +} + +float signedDistanceToPolygon(vec2 p) { + float dist = distanceToEdge(p); + bool inside = isPointInsidePolygon(p); + return inside ? dist : -dist; +} + +void main() { + vec2 pixel = fragTexCoord * resolution; + + float signedDist = signedDistanceToPolygon(pixel); + + vec2 pixelGrad = vec2(dFdx(pixel.x), dFdy(pixel.y)); + float pixelSize = length(pixelGrad); + float aaWidth = max(0.5, pixelSize * 1.0); + + float alpha = smoothstep(-aaWidth, aaWidth, signedDist); + + if (alpha > 0.0) { + vec4 color; + if (useGradient) { + color = getGradientColor(fragTexCoord); + } else { + color = fillColor; + } + finalColor = vec4(color.rgb, color.a * alpha); + } else { + finalColor = vec4(0.0, 0.0, 0.0, 0.0); + } +} +""" + +# Default vertex shader +VERTEX_SHADER = """ +#version 300 es +in vec3 vertexPosition; +in vec2 vertexTexCoord; +out vec2 fragTexCoord; +uniform mat4 mvp; + +void main() { + fragTexCoord = vertexTexCoord; + gl_Position = mvp * vec4(vertexPosition, 1.0); +} +""" + + +class ShaderState: + _instance: Any = None + + @classmethod + def get_instance(cls): + if cls._instance is None: + cls._instance = cls() + return cls._instance + + def __init__(self): + if ShaderState._instance is not None: + raise Exception("This class is a singleton. Use get_instance() instead.") + + self.initialized = False + self.shader = None + self.white_texture = None + + # Shader uniform locations + self.locations = { + 'pointCount': None, + 'fillColor': None, + 'resolution': None, + 'points': None, + 'useGradient': None, + 'gradientStart': None, + 'gradientEnd': None, + 'gradientColors': None, + 'gradientStops': None, + 'gradientColorCount': None, + 'mvp': None, + } + + def initialize(self): + if self.initialized: + return + + vertex_shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAGMENT_SHADER) + self.shader = vertex_shader + + # Create and cache white texture + white_img = rl.gen_image_color(2, 2, rl.WHITE) + self.white_texture = rl.load_texture_from_image(white_img) + rl.set_texture_filter(self.white_texture, rl.TEXTURE_FILTER_BILINEAR) + rl.unload_image(white_img) + + # Cache all uniform locations + for uniform in self.locations.keys(): + self.locations[uniform] = rl.get_shader_location(self.shader, uniform) + + # Setup default MVP matrix + mvp_ptr = rl.ffi.new("float[16]", [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]) + rl.set_shader_value_matrix(self.shader, self.locations['mvp'], rl.Matrix(*mvp_ptr)) + + self.initialized = True + + def cleanup(self): + if not self.initialized: + return + + if self.white_texture: + rl.unload_texture(self.white_texture) + self.white_texture = None + + if self.shader: + rl.unload_shader(self.shader) + self.shader = None + + self.initialized = False + + +def draw_polygon(points: np.ndarray, color=None, gradient=None): + """ + Draw a complex polygon using shader-based even-odd fill rule + + Args: + points: List of (x,y) points defining the polygon + color: Solid fill color (rl.Color) + gradient: Dict with gradient parameters: + { + 'start': (x1, y1), # Start point (normalized 0-1) + 'end': (x2, y2), # End point (normalized 0-1) + 'colors': [rl.Color], # List of colors at stops + 'stops': [float] # List of positions (0-1) + } + """ + if len(points) < 3: + return + + # Get shader state singleton + state = ShaderState.get_instance() + + # Initialize shader if not already done + if not state.initialized: + state.initialize() + + # Find bounding box + min_xy = np.min(points, axis=0) + min_x, min_y = min_xy + max_x, max_y = np.max(points, axis=0) + + width = max(1, max_x - min_x) + height = max(1, max_y - min_y) + + # Transform points to shader space + transformed_points = points - min_xy + + # Set basic shader uniforms using cached locations + point_count_ptr = rl.ffi.new("int[]", [len(transformed_points)]) + rl.set_shader_value(state.shader, state.locations['pointCount'], point_count_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + + resolution_ptr = rl.ffi.new("float[]", [width, height]) + rl.set_shader_value(state.shader, state.locations['resolution'], resolution_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) + + # Set points + flat_points = np.ascontiguousarray(transformed_points.flatten().astype(np.float32)) + points_ptr = rl.ffi.cast("float *", flat_points.ctypes.data) + rl.set_shader_value_v( + state.shader, state.locations['points'], points_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2, len(transformed_points) + ) + + # Set gradient or solid color based on what was provided + if gradient: + # Enable gradient + use_gradient_ptr = rl.ffi.new("int[]", [1]) + rl.set_shader_value(state.shader, state.locations['useGradient'], use_gradient_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + + # Set gradient start/end + start_ptr = rl.ffi.new("float[]", [gradient['start'][0], gradient['start'][1]]) + end_ptr = rl.ffi.new("float[]", [gradient['end'][0], gradient['end'][1]]) + rl.set_shader_value(state.shader, state.locations['gradientStart'], start_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) + rl.set_shader_value(state.shader, state.locations['gradientEnd'], end_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) + + # Set gradient colors + colors = gradient['colors'] + color_count = min(len(colors), 8) # Max 8 colors + colors_ptr = rl.ffi.new("float[]", color_count * 4) + for i, c in enumerate(colors[:color_count]): + colors_ptr[i * 4] = c.r / 255.0 + colors_ptr[i * 4 + 1] = c.g / 255.0 + colors_ptr[i * 4 + 2] = c.b / 255.0 + colors_ptr[i * 4 + 3] = c.a / 255.0 + rl.set_shader_value_v( + state.shader, state.locations['gradientColors'], colors_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC4, color_count + ) + + # Set gradient stops + stops = gradient.get('stops', [i / (color_count - 1) for i in range(color_count)]) + stops_ptr = rl.ffi.new("float[]", color_count) + for i, s in enumerate(stops[:color_count]): + stops_ptr[i] = s + rl.set_shader_value_v( + state.shader, state.locations['gradientStops'], stops_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_FLOAT, color_count + ) + + # Set color count + color_count_ptr = rl.ffi.new("int[]", [color_count]) + rl.set_shader_value(state.shader, state.locations['gradientColorCount'], color_count_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + else: + # Disable gradient + use_gradient_ptr = rl.ffi.new("int[]", [0]) + rl.set_shader_value(state.shader, state.locations['useGradient'], use_gradient_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + + # Set solid color + if color is None: + color = rl.WHITE + fill_color_ptr = rl.ffi.new("float[]", [color.r / 255.0, color.g / 255.0, color.b / 255.0, color.a / 255.0]) + rl.set_shader_value(state.shader, state.locations['fillColor'], fill_color_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC4) + + # Draw with shader + rl.begin_shader_mode(state.shader) + rl.draw_texture_pro( + state.white_texture, + rl.Rectangle(0, 0, 2, 2), + rl.Rectangle(int(min_x), int(min_y), int(width), int(height)), + rl.Vector2(0, 0), + 0.0, + rl.WHITE, + ) + rl.end_shader_mode() + + +def cleanup_shader_resources(): + state = ShaderState.get_instance() + state.cleanup() diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index d0b66666fa..9060dd3f74 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -4,6 +4,7 @@ import numpy as np import pyray as rl from cereal import messaging, car from openpilot.common.params import Params +from openpilot.system.ui.lib.shader_polygon import draw_polygon CLIP_MARGIN = 500 @@ -30,14 +31,14 @@ class ModelRenderer: self._experimental_mode = False self._blend_factor = 1.0 self._prev_allow_throttle = True - self._lane_line_probs = [0.0] * 4 - self._road_edge_stds = [0.0] * 2 + self._lane_line_probs = np.zeros(4, dtype=np.float32) + self._road_edge_stds = np.zeros(2, dtype=np.float32) self._path_offset_z = 1.22 # Initialize empty polygon vertices - self._track_vertices = [] - self._lane_line_vertices = [[] for _ in range(4)] - self._road_edge_vertices = [[] for _ in range(2)] + self._track_vertices = np.empty((0, 2), dtype=np.float32) + self._lane_line_vertices = [np.empty((0, 2), dtype=np.float32) for _ in range(4)] + self._road_edge_vertices = [np.empty((0, 2), dtype=np.float32) for _ in range(2)] self._lead_vertices = [None, None] # Transform matrix (3x3 for car space to screen space) @@ -145,29 +146,29 @@ class ModelRenderer: def _draw_lane_lines(self): """Draw lane lines and road edges""" - for i in range(4): + for i, vertices in enumerate(self._lane_line_vertices): # Skip if no vertices - if not self._lane_line_vertices[i]: + if vertices.size == 0: continue # Draw lane line alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7) color = rl.Color(255, 255, 255, int(alpha * 255)) - self._draw_polygon(self._lane_line_vertices[i], color) + draw_polygon(vertices, color) - for i in range(2): + for i, vertices in enumerate(self._road_edge_vertices): # Skip if no vertices - if not self._road_edge_vertices[i]: + if vertices.size == 0: continue # Draw road edge alpha = np.clip(1.0 - self._road_edge_stds[i], 0.0, 1.0) color = rl.Color(255, 0, 0, int(alpha * 255)) - self._draw_polygon(self._road_edge_vertices[i], color) + draw_polygon(vertices, color) def _draw_path(self, sm, model, height): """Draw the path polygon with gradient based on acceleration""" - if not self._track_vertices: + if self._track_vertices.size == 0: return if self._experimental_mode: @@ -175,16 +176,29 @@ class ModelRenderer: acceleration = model.acceleration.x max_len = min(len(self._track_vertices) // 2, len(acceleration)) - # Create gradient colors for path sections - for i in range(max_len): + # Find midpoint index for polygon + mid_point = len(self._track_vertices) // 2 + + # For acceleration-based coloring, process segments separately + left_side = self._track_vertices[:mid_point] + right_side = self._track_vertices[mid_point:][::-1] # Reverse for proper winding + + # Create segments for gradient coloring + segment_colors = [] + gradient_stops = [] + + for i in range(max_len - 1): + if i >= len(left_side) - 1 or i >= len(right_side) - 1: + break + track_idx = max_len - i - 1 # flip idx to start from bottom right - track_y = self._track_vertices[track_idx][1] + # Skip points out of frame - if track_y < 0 or track_y > height: + if left_side[track_idx][1] < 0 or left_side[track_idx][1] > height: continue # Calculate color based on acceleration - lin_grad_point = (height - track_y) / height + lin_grad_point = (height - left_side[track_idx][1]) / height # speed up: 120, slow down: 0 path_hue = max(min(60 + acceleration[i] * 35, 120), 0) @@ -197,12 +211,22 @@ class ModelRenderer: # Use HSL to RGB conversion color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha) - # TODO: This is simplified - a full implementation would create a gradient fill - segment = self._track_vertices[track_idx : track_idx + 2] + self._track_vertices[-track_idx - 2 : -track_idx] - self._draw_polygon(segment, color) - - # Skip a point, unless next is last - i += 1 if i + 2 < max_len else 0 + # Create quad segment + gradient_stops.append(lin_grad_point) + segment_colors.append(color) + + if len(segment_colors) < 2: + draw_polygon(self._track_vertices, rl.Color(255, 255, 255, 30)) + return + + # Create gradient specification + gradient = { + 'start': (0.0, 1.0), # Bottom of path + 'end': (0.0, 0.0), # Top of path + 'colors': segment_colors, + 'stops': gradient_stops, + } + draw_polygon(self._track_vertices, gradient=gradient) else: # Draw with throttle/no throttle gradient allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control @@ -226,7 +250,13 @@ class ModelRenderer: self._blend_colors(begin_colors[2], end_colors[2], self._blend_factor), ] - self._draw_polygon(self._track_vertices, colors[0]) + gradient = { + 'start': (0.0, 1.0), # Bottom of path + 'end': (0.0, 0.0), # Top of path + 'colors': colors, + 'stops': [0.0, 1.0], + } + draw_polygon(self._track_vertices, gradient=gradient) def _draw_lead(self, lead_data, vd, rect): """Draw lead vehicle indicator""" @@ -284,14 +314,14 @@ class ModelRenderer: return (x, y) - def _map_line_to_polygon(self, line, y_off, z_off, max_idx, allow_invert=True): + def _map_line_to_polygon(self, line, y_off, z_off, max_idx, allow_invert=True)-> np.ndarray: """Convert a 3D line to a 2D polygon for drawing""" line_x = line.x line_y = line.y line_z = line.z - left_points = [] - right_points = [] + left_points: list[tuple[float, float]] = [] + right_points: list[tuple[float, float]] = [] for i in range(max_idx + 1): # Skip points with negative x (behind camera) @@ -309,23 +339,10 @@ class ModelRenderer: left_points.append(left) right_points.append(right) - if not left_points: - return [] - - return left_points + right_points[::-1] - - def _draw_polygon(self, points, color): - # TODO: Enhance polygon drawing to support even-odd fill rule efficiently, as Raylib lacks native support. - # Use a faster triangulation algorithm (e.g., ear clipping) or GPU shader for - # efficient rendering of lane lines, road edges, and path polygons. - if len(points) <= 8: - rl.draw_triangle_fan(points, len(points), color) - else: - for i in range(1, len(points) - 1): - rl.draw_triangle(points[0], points[i], points[i + 1], color) + if not left_points or not right_points: + return np.empty((0, 2), dtype=np.float32) - for i in range(len(points)): - rl.draw_line_ex(points[i], points[(i + 1) % len(points)], 1.5, color) + return np.array(left_points + right_points[::-1], dtype=np.float32) @staticmethod def _map_val(x, x0, x1, y0, y1):