def _find_shortest_path(self, start_position: Position,
goal_position: Position):
if self._maze is None:
raise InvalidMazeException
start = start_position.to_tuple()
goal = goal_position.to_tuple()
start_x, start_y = start
goal_x, goal_y = goal
size_x, size_y = self.get_dimensions()
if start_x not in range(0, size_x + 1) or start_y not in range(
0, size_y + 1):
raise InvalidStartPointException
if goal_x not in range(0, size_x + 1) or goal_y not in range(
0, size_y + 1):
raise InvalidDestinationException
if self._maze[start_x][start_y] == 1:
raise InvalidStartPointException
if self._maze[goal_x][goal_y] == 1:
raise InvalidDestinationException
came_from = self._a_star_search(start, goal)
path = self._reconstruct_path(came_from, start, goal)
return path
def calculate_distance_between_two_positions(first_position: Position,
second_position: Position):
first_x, first_y = first_position.to_tuple()
second_x, second_y = second_position.to_tuple()
distance = math.sqrt((first_x - second_x)**2 + (first_y - second_y)**2)
return Distance(distance)
def calculate_angle_between_positions(
first_position: Position,
second_position: Position) -> Orientation:
first_x, first_y = first_position.to_tuple()
second_x, second_y = second_position.to_tuple()
angle = -math.atan2(second_y - first_y, second_x - first_x)
if angle < 0:
angle += 2 * math.pi
return Orientation.from_radian(angle)
def add_obstacle(self, obstacle_position: Position, robot_radius: int,
obstacle_radius) -> None:
x_coordinate, y_coordinate = obstacle_position.to_tuple()
total_obstacle_radius = robot_radius + obstacle_radius
for i in range(-total_obstacle_radius, total_obstacle_radius + 1):
for j in range(-total_obstacle_radius, total_obstacle_radius + 1):
if i + j <= total_obstacle_radius * 2:
try:
self._array[y_coordinate + i][x_coordinate + j] = 1
except IndexError:
continue
def remove_puck_as_obstacle(self,
obstacle_position: Position,
robot_radius=0,
obstacle_radius=85):
x_coordinate, y_coordinate = obstacle_position.to_tuple()
total_obstacle_radius = robot_radius + obstacle_radius
for i in range(-total_obstacle_radius, total_obstacle_radius + 1):
for j in range(-total_obstacle_radius, total_obstacle_radius + 1):
if i + j <= total_obstacle_radius * 2:
try:
self._array[y_coordinate + i][x_coordinate + j] = 0
except IndexError:
continue
