def get_possible_moves(self):
available_points = []
(a_y, a_x) = self.current_pos
for x in range(a_x - self.max_step, a_x + self.max_step + 1):
for y in range(a_y - self.max_step, a_y + self.max_step + 1):
if y >= len(self.collision_map) or x >= len(
self.collision_map[y]):
continue
if self.collision_map[y][x] == 0:
distance = nav.get_distance_beteween_points(
self.current_pos, (y, x))
angle = nav.get_angle_of_direction_between_points(
self.current_pos, (y, x))
desired_angle = self.direction_map.get_angle(
self.current_pos)
angle_diff = abs(angle - desired_angle)
if angle_diff > 3 / 2 * np.pi:
angle_diff = abs(angle_diff - np.pi)
if distance <= self.max_step and angle_diff <= self.forward_move_angle / 2:
available_points.append((y, x))
return available_points
def move_price(self, pos: (int, int)) -> float:
if self.direction_map.direction_map[pos[0]][pos[1]] == Env.EXIT:
return 256
price = (nav.get_distance_beteween_points(
self.direction_map.get_next_position(self.current_pos), pos))
#price=price*nav.get_distance_beteween_points(pos,self.end[0])
return np.floor(price)
def mark_circle_obstacle(center_location: (int, int), radius: int,
collision_map: [[int]], marking_value: int):
(loc_y, loc_x) = center_location
for x in range(loc_x - radius, loc_x + radius + 1):
for y in range(loc_y - radius, loc_y + radius + 1):
distance = nav.get_distance_beteween_points(
center_location, (y, x))
if distance <= radius:
mark_location((y, x), collision_map, marking_value)
def get_step_size(self, current_pos: (int, int)):
return nav.get_distance_beteween_points(
current_pos, self.get_next_position(current_pos))
def test_distane_cross(self):
self.assertEqual(nav.get_distance_beteween_points((2, 2), (3, 7)),
(1**2 + 5**2)**(1 / 2), "Distance between points")
def test_distane(self):
self.assertEqual(nav.get_distance_beteween_points((0, 0), (3, 4)), 5,
"Distance between points")
def test_geting_step_size(self):
self.directions[2][5] = (12, 12)
direction_map = DirectionMap(self.directions)
step = nav.get_distance_beteween_points((2, 5), (12, 12))
self.assertEqual(step, direction_map.get_step_size((2, 5)))