def right_button_pressed(self, chosen_point): chose_backwards_point = False if self._backwards_point: backwards_distance = geometry.get_distance_between_points( chosen_point, self._backwards_point) primary_distance = geometry.get_distance_between_points( chosen_point, self._chosen_point) chose_backwards_point = backwards_distance < 0.5 * primary_distance and abs( primary_distance - BACKWARDS_DISTANCE) < 0.25 * BACKWARDS_DISTANCE if chose_backwards_point: self._backwards_point = chosen_point self._chosen_bearing = geometry.get_bearing_between_points( self._backwards_point, self._chosen_point) else: self._chosen_point, waypoint_id = self.current_track.get_adjusted_point_on_track( chosen_point) self._chosen_bearing = self.current_track.get_bearing_at_waypoint( waypoint_id) self._backwards_point = geometry.get_point_at_bearing( self._chosen_point, self._chosen_bearing - 180, BACKWARDS_DISTANCE) self.guru_parent_redraw()
def _get_closest_waypoint_id(self, point): distance = geometry.get_distance_between_points( self._track_waypoints[0], point) closest_id = 0 for i, w in enumerate(self._track_waypoints[1:]): new_distance = geometry.get_distance_between_points(w, point) if new_distance < distance: distance = new_distance closest_id = i + 1 return closest_id
def get_position_of_point_relative_to_waypoint(self, point: Point, waypoint_id: int): dp = self._drawing_points[waypoint_id] left_distance = geometry.get_distance_between_points(point, dp.left) right_distance = geometry.get_distance_between_points(point, dp.right) if abs(left_distance - right_distance) < 0.001: return "C" elif left_distance < right_distance: return "L" else: return "R"
def get_closest_waypoint_id(self, point: Point): best_distance = geometry.get_distance_between_points( point, self._drawing_points[0].middle) best_waypoint = 0 for i, p in enumerate(self._drawing_points): distance = min( geometry.get_distance_between_points(point, p.left), geometry.get_distance_between_points(point, p.right), geometry.get_distance_between_points(point, p.middle)) if distance < best_distance: best_distance = distance best_waypoint = i return best_waypoint
def _set_distance_from_center_on_events(self, track: Track): for e in self.events: current_location = (e.x, e.y) closest_waypoint = track.get_waypoint(e.closest_waypoint_index) next_waypoint = track.get_next_different_waypoint( e.closest_waypoint_index) previous_waypoint = track.get_previous_different_waypoint( e.closest_waypoint_index) distance_of_next_waypoint = get_distance_between_points( current_location, next_waypoint) distance_of_previous_waypoint = get_distance_between_points( current_location, previous_waypoint) if distance_of_next_waypoint < distance_of_previous_waypoint: e.distance_from_center = get_distance_of_point_from_line( current_location, closest_waypoint, next_waypoint) else: e.distance_from_center = get_distance_of_point_from_line( current_location, closest_waypoint, previous_waypoint)
def get_bearing_and_distance_to_next_waypoint(self, waypoint_id: int): this_point = self._drawing_points[waypoint_id].middle next_point = self._drawing_points[self._get_next_waypoint_id( waypoint_id)].middle distance = geometry.get_distance_between_points(this_point, next_point) bearing = geometry.get_bearing_between_points(this_point, next_point) return bearing, distance
def _calculate_distances(self): previous = self._drawing_points[-1] for p in self._drawing_points: self._measured_left_distance += geometry.get_distance_between_points( previous.left, p.left) self._measured_middle_distance += geometry.get_distance_between_points( previous.middle, p.middle) self._measured_right_distance += geometry.get_distance_between_points( previous.right, p.right) previous = p previous = self._drawing_points[0] progress_distance = 0.0 for p in self._drawing_points: progress_distance += geometry.get_distance_between_points( previous.middle, p.middle) self._percent_from_race_start.append( round(progress_distance / self._measured_middle_distance * 100, 2)) previous = p
def _mark_dodgy_data(self): e: Event for i, e in enumerate(self.events[1:-1]): previous_event = self.events[i] previous_point = (previous_event.x, previous_event.y) next_event = self.events[i + 2] next_point = (next_event.x, next_event.y) current_point = (e.x, e.y) distance_to_previous = get_distance_between_points( current_point, previous_point) distance_to_next = get_distance_between_points( current_point, next_point) time_gap_to_previous = e.time - previous_event.time time_gap_to_next = next_event.time - e.time if time_gap_to_previous > 3 * time_gap_to_next: e.dodgy_data = True elif max(distance_to_next, distance_to_previous) > 0.1: if distance_to_next > 2 * distance_to_previous or distance_to_previous > 2 * distance_to_next: e.dodgy_data = True
def get_waypoint_ids_before_and_after(self, point: Point, closest_waypoint_id: int, prefer_forwards=False): assert 0 <= closest_waypoint_id < len(self._track_waypoints) previous_id = self._get_previous_waypoint_id(closest_waypoint_id) next_id = self._get_next_waypoint_id(closest_waypoint_id) previous_waypoint = self._drawing_points[previous_id].middle next_waypoint = self._drawing_points[next_id].middle closest_waypoint = self._drawing_points[closest_waypoint_id].middle target_dist = geometry.get_distance_between_points( closest_waypoint, previous_waypoint) if target_dist == 0.0: previous_ratio = 99999.0 else: previous_ratio = geometry.get_distance_between_points( point, previous_waypoint) / target_dist target_dist = geometry.get_distance_between_points( closest_waypoint, next_waypoint) if target_dist == 0.0: next_ratio = 99999.0 else: next_ratio = geometry.get_distance_between_points( point, next_waypoint) / target_dist if prefer_forwards: # Make the behind waypoint appear 5% further away previous_ratio *= 1.05 if previous_ratio > next_ratio: return closest_waypoint_id, next_id else: return previous_id, closest_waypoint_id
def draw_waypoint_labels(self, track_graphics: TrackGraphics, colour: str, font_size: int): last_label_position = None for (i, p) in enumerate(self._drawing_points[:-2]): if self._is_vertical_at_waypoint(i): label = track_graphics.plot_text(p.middle, str(i), font_size, colour, -1.5 * font_size, 0.0) else: label = track_graphics.plot_text(p.middle, str(i), font_size, colour, 0.0, 1.5 * font_size) label_position = track_graphics.get_widget_position(label) if last_label_position is None: last_label_position = label_position elif geometry.get_distance_between_points( last_label_position, label_position) < 2.5 * font_size: track_graphics.delete_widget(label) else: last_label_position = label_position
def get_adjusted_point_on_track(self, chosen_point: Point, margin: float = 0.0): waypoint_id = self.get_closest_waypoint_id(chosen_point) waypoint = self.get_waypoint(waypoint_id) distance_from_waypoint = geometry.get_distance_between_points( waypoint, chosen_point) max_distance_from_centre = (self.get_width() + VEHICLE_WIDTH) / 2 - margin if distance_from_waypoint > max_distance_from_centre: bearing_of_point = geometry.get_bearing_between_points( waypoint, chosen_point) adjusted_point = geometry.get_point_at_bearing( waypoint, bearing_of_point, max_distance_from_centre) else: adjusted_point = chosen_point return adjusted_point, waypoint_id
def set_track_speed_on_events(self): previous = [ self.events[0] ] * 6 # 6 here matches DRF, but (TODO) DRF is marginally more accurate algorithm improve_previous = False for e in self.events: if e.dodgy_data or previous[0].dodgy_data: e.track_speed = previous[-1].track_speed improve_previous = True else: distance = get_distance_between_points( (e.x, e.y), (previous[0].x, previous[0].y)) time_taken = e.time - previous[0].time if time_taken > 0: e.track_speed = distance / time_taken if e.track_speed > self.peak_track_speed: self.peak_track_speed = e.track_speed if improve_previous: previous[-1].track_speed = ( e.track_speed + previous[-1].track_speed) / 2 improve_previous = False previous = previous[1:] + [e]
def _process_raw_waypoints(self): previous = self._track_waypoints[ -2] # Must be penultimate since last one is same as first one (self._min_x, self._min_y) = previous (self._max_x, self._max_y) = previous left = previous right = previous left_outer = previous right_outer = previous left_safe = previous right_safe = previous edge_error_tolerance = 0.01 safe_car_overhang = min(VEHICLE_LENGTH, VEHICLE_WIDTH) / 2 outer_distance = 0.08 for i, w in enumerate(self._track_waypoints + [self._track_waypoints[0]]): # Tracks often contain a repeated waypoint, suspect this is deliberate to mess up waypoint algorithms! if previous != w: if i < len(self._track_waypoints) - 1: future = self._track_waypoints[i + 1] else: future = self._track_waypoints[0] previous_left = left previous_right = right left = geometry.get_edge_point(previous, w, future, 90, self._track_width / 2) if geometry.get_distance_between_points( previous_left, left) < edge_error_tolerance: left = previous_left else: left_outer = geometry.get_edge_point( previous, w, future, 90, self._track_width / 2 + outer_distance) left_safe = geometry.get_edge_point( previous, w, future, 90, self._track_width / 2 + safe_car_overhang) right = geometry.get_edge_point(previous, w, future, -90, self._track_width / 2) if geometry.get_distance_between_points( previous_right, right) < edge_error_tolerance: right = previous_right else: right_outer = geometry.get_edge_point( previous, w, future, -90, self._track_width / 2 + outer_distance) right_safe = geometry.get_edge_point( previous, w, future, -90, self._track_width / 2 + safe_car_overhang) self._consider_new_point_in_area(left_outer) self._consider_new_point_in_area(w) self._consider_new_point_in_area(right_outer) previous = w is_divider = (i in self._track_sector_dividers) self._drawing_points.append( Track.DrawingPoint(left, w, right, left_outer, right_outer, left_safe, right_safe, is_divider)) self._mid_x = (self._min_x + self._max_x) / 2 self._mid_y = (self._min_y + self._max_y) / 2
def get_projected_distance_on_track(self, point: Point, heading: float, closest_waypoint_id: int, path_width: float = 0.0): heading = geometry.get_angle_in_proper_range(heading) if path_width > 0.0: side_point_1 = geometry.get_point_at_bearing( point, heading + 90, path_width / 2) side_point_2 = geometry.get_point_at_bearing( point, heading - 90, path_width / 2) return min( self.get_projected_distance_on_track(point, heading, closest_waypoint_id), self.get_projected_distance_on_track(side_point_1, heading, closest_waypoint_id), self.get_projected_distance_on_track(side_point_2, heading, closest_waypoint_id)) (before_waypoint_id, after_waypoint_id) = self.get_waypoint_ids_before_and_after( point, closest_waypoint_id, True) previous_left = self._drawing_points[before_waypoint_id].left_safe previous_right = self._drawing_points[before_waypoint_id].right_safe for w in self._drawing_points[ after_waypoint_id:] + self._drawing_points[:after_waypoint_id]: direction_to_left_target = geometry.get_bearing_between_points( point, w.left_safe) direction_to_right_target = geometry.get_bearing_between_points( point, w.right_safe) relative_direction_to_left_target = geometry.get_turn_between_directions( heading, direction_to_left_target) relative_direction_to_right_target = geometry.get_turn_between_directions( heading, direction_to_right_target) if relative_direction_to_left_target >= 0 and relative_direction_to_right_target <= 0: previous_left = w.left_safe previous_right = w.right_safe else: point2 = geometry.get_point_at_bearing( point, heading, 1) # Just some random distance (1m) if w.left_safe == previous_left: off_track_left = previous_left else: off_track_left = geometry.get_intersection_of_two_lines( point, point2, w.left_safe, previous_left) if w.right_safe == previous_right: off_track_right = previous_right else: off_track_right = geometry.get_intersection_of_two_lines( point, point2, w.right_safe, previous_right) left_bearing = geometry.get_bearing_between_points( point, off_track_left) right_bearing = geometry.get_bearing_between_points( point, off_track_right) distances = [] if abs( geometry.get_turn_between_directions( left_bearing, heading)) < 1: if geometry.is_point_between(off_track_left, w.left_safe, previous_left): distances += [ geometry.get_distance_between_points( point, off_track_left) ] if abs( geometry.get_turn_between_directions( right_bearing, heading)) < 1: if geometry.is_point_between(off_track_right, w.right_safe, previous_right): distances += [ geometry.get_distance_between_points( point, off_track_right) ] if len(distances) > 0: return max(distances) else: return 0.0