def draw_ground(cls, lane: AbstractLane, surface: WorldSurface, color: Tuple[float], width: float, draw_surface: pygame.Surface = None) -> None: draw_surface = draw_surface or surface stripes_count = int(2 * (surface.get_height() + surface.get_width()) / (cls.STRIPE_SPACING * surface.scaling)) s_origin, _ = lane.local_coordinates(surface.origin) s0 = (int(s_origin) // cls.STRIPE_SPACING - stripes_count // 2) * cls.STRIPE_SPACING dots = [] for side in range(2): longis = np.clip( s0 + np.arange(stripes_count) * cls.STRIPE_SPACING, 0, lane.length) lats = [2 * (side - 0.5) * width for _ in longis] new_dots = [ surface.vec2pix(lane.position(longi, lat)) for longi, lat in zip(longis, lats) ] new_dots = reversed(new_dots) if side else new_dots dots.extend(new_dots) pygame.draw.polygon(draw_surface, color, dots, 0)
def draw_stripes(cls, lane: AbstractLane, surface: WorldSurface, starts: List[float], ends: List[float], lats: List[float]) -> None: """ Draw a set of stripes along a lane. :param lane: the lane :param surface: the surface to draw on :param starts: a list of starting longitudinal positions for each stripe [m] :param ends: a list of ending longitudinal positions for each stripe [m] :param lats: a list of lateral positions for each stripe [m] """ starts = np.clip(starts, 0, lane.length) ends = np.clip(ends, 0, lane.length) for k, _ in enumerate(starts): if abs(starts[k] - ends[k]) > 0.5 * cls.STRIPE_LENGTH: pygame.draw.line(surface, surface.WHITE, (surface.vec2pix(lane.position(starts[k], lats[k]))), (surface.vec2pix(lane.position(ends[k], lats[k]))), max(surface.pix(cls.STRIPE_WIDTH), 1))
def interval_local_to_absolute(longitudinal_i: Interval, lateral_i: Interval, lane: AbstractLane) -> Interval: """ Converts an interval in local (longiturinal, lateral) coordinates to an interval in absolute x,y coordinates :param longitudinal_i: the longitudinal interval [L_min, L_max] :param lateral_i: the lateral interval [l_min, l_max] :param lane: the lane giving the local frame :return: the corresponding absolute interval """ corners_local = [[longitudinal_i[0], lateral_i[0]], [longitudinal_i[0], lateral_i[1]], [longitudinal_i[1], lateral_i[0]], [longitudinal_i[1], lateral_i[1]]] corners_absolute = np.array([lane.position(*c) for c in corners_local]) position_i = np.array([np.amin(corners_absolute, axis=0), np.amax(corners_absolute, axis=0)]) return position_i