diff --git a/src/environment/a_star.py b/src/environment/a_star.py index 5b902ab..314e0c8 100644 --- a/src/environment/a_star.py +++ b/src/environment/a_star.py @@ -1,117 +1,193 @@ -class Node: - """A node class for A* Pathfinding""" +from .line import Point - def __init__(self, parent=None, position=None): - self.parent = parent - self.position = position +import numpy - self.g = 0 - self.h = 0 - self.f = 0 - def __eq__(self, other): - return self.position == other.position +def a_star(environment, start, end): # (int[][], Point, Point) + if len(environment) < start.y or len(environment[0]) < start.x or start.x < 0 or start.y < 0: + raise PointOutOfEnvironmentRangeException( + "Point (start: " + str(start) + ") out of environment range " + + str(len(environment)) + "x" + str(len(environment[0]))) + if len(environment) < end.y or len(environment[0]) < end.x or end.x < 0 or end.y < 0: + raise PointOutOfEnvironmentRangeException( + "Point (end: " + str(end) + ") out of environment range " + + str(len(environment)) + "x" + str(len(environment[0]))) -def astar(maze, start, end): - """Returns a list of tuples as a path from the given start to the given end in the given maze""" - - # Create start and end node + visited = [] + non_visited = [] start_node = Node(None, start) - start_node.g = start_node.h = start_node.f = 0 - end_node = Node(None, end) - end_node.g = end_node.h = end_node.f = 0 - - # Initialize both open and closed list - open_list = [] - closed_list = [] - - # Add the start node - open_list.append(start_node) - - # Loop until you find the end - while len(open_list) > 0: - - # Get the current node - current_node = open_list[0] - current_index = 0 - for index, item in enumerate(open_list): - if item.f < current_node.f: - current_node = item - current_index = index - - # Pop current off open list, add to closed list - open_list.pop(current_index) - closed_list.append(current_node) - - # Found the goal - if current_node == end_node: - path = [] - current = current_node - while current is not None: - path.append(current.position) - current = current.parent - return path[::-1] # Return reversed path - - # Generate children - children = [] - for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)]: # Adjacent squares - - # Get node position - node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) - - # Make sure within range - if node_position[0] > (len(maze) - 1) or node_position[0] < 0 or node_position[1] > (len(maze[len(maze)-1]) -1) or node_position[1] < 0: - continue - - # Make sure walkable terrain - if maze[node_position[0]][node_position[1]] != 0: - continue - - # Create new node - new_node = Node(current_node, node_position) - - # Append - children.append(new_node) - - # Loop through children - for child in children: - - # Child is on the closed list - for closed_child in closed_list: - if child == closed_child: + non_visited.extend(get_neighbours(environment, start_node, end, [])) + non_visited.sort() + + while len(non_visited) > 0: + lowest_cost_node = non_visited[0] + if lowest_cost_node.cords == end: + return reconstruct_path(lowest_cost_node) + + non_visited.remove(lowest_cost_node) + visited.append(lowest_cost_node) + + neighbours = get_neighbours(environment, lowest_cost_node, end, visited) + for node in neighbours: + update = update_non_visited(node, non_visited) + if update is not None: + if update is True: + non_visited.append(node) + else: continue + else: + non_visited.append(node) - # Create the f, g, and h values - child.g = current_node.g + 1 - child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2) - child.f = child.g + child.h + non_visited.sort() - # Child is already in the open list - for open_node in open_list: - if child == open_node and child.g > open_node.g: - continue + return False + + +def reconstruct_path(node): # Point[] / Node + path = [] + while node.parent is not None: + path.append(node.cords) + node = node.parent + path.append(node.cords) + path.reverse() + return path + + +def already_visited(cords, nodes): # Point, Node[]) + for node in nodes: + if cords == node.cords: + return True + return False + + +def get_neighbours(environment, node, end, visited): # Node[] / (Node, Point) + """ get all the node correct neighbours """ + + neighbours_cords = [ + Point(-1, -1), Point(0, -1), Point(1, -1), + Point(-1, 0), Point(1, 0), + Point(-1, 1), Point(0, 1), Point(1, 1), + ] + + environment_x_range = len(environment[0]) + environment_y_range = len(environment) + + neighbours_nodes = [] + for cords in neighbours_cords: + + # if it is parent node we skip + if node.parent is not None and cords+node.cords == node.parent.cords: + continue + + # if we move vertically or horizontal we move 1, if diagonal sqrt(2) + diagonal_node = True + if cords.y == 0 or cords.x == 0: + diagonal_node = False + + cords += node.cords + + # check if the cord is out of environment range + if cords.x >= environment_x_range or cords.x < 0 or cords.y >= environment_y_range or cords.y < 0: + continue - # Add the child to the open list - open_list.append(child) + # check if this cord is an obstacle (1 mean it is) + if environment[cords.y][cords.x] == 1: + continue + + # check if this cords have been already visited + if already_visited(cords, visited): + continue + + neighbour = Node(node, cords) + neighbour.g_cost = node.g_cost + (numpy.sqrt(2) if diagonal_node else 1) + neighbour.h_cost = diagonal_distance_heuristics(cords, end) + + # check if already same cords was seen, if yes and if new node is better remove old + # for seen_node in non_visited: + # if node.cords == seen_node.cords: + # if node <= seen_node: + # non_visited.remove(seen_node) + # else: + # continue + + neighbours_nodes.append(neighbour) + + return neighbours_nodes + + +def update_non_visited(node, non_visited): # (Node, Node[]) + for seen_node in non_visited: + if node.cords == seen_node.cords: + if node <= seen_node: + non_visited.remove(seen_node) + return True + else: + return False + return None + + +def diagonal_distance_heuristics(current, end): # (Point, Point) + """ we have to use different heuristic since we can move just in 8 direction, more info: + http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#diagonal-distance""" + + # d = 1 + d2 = numpy.sqrt(2) + dx = numpy.abs(end.x - current.x) + dy = numpy.abs(end.y - current.y) + + if dx > dy: + return (dx - dy) + d2 * dy + else: + return (dy - dx) + d2 * dx + + +# TODO change compare (bad code) +class Node: + def __init__(self, parent, cords): + self.parent = parent + self.cords = cords + self.g_cost = 0 # cost from start to current + self.h_cost = 0 # cost from current to end + + def f_cost(self): + return self.g_cost+self.h_cost + + def __cmp__(self, other): + if self.f_cost() > other.f_cost(): + return 1 + elif self.f_cost() < other.f_cost(): + return -1 + else: + if self.h_cost > other.h_cost: + return 1 + elif self.h_cost < other.h_cost: + return -1 + else: + return 0 + + def __lt__(self, other): + return self.__cmp__(other) < 0 + + def __gt__(self, other): + return self.__cmp__(other) > 0 + + def __eq__(self, other): + return self.__cmp__(other) == 0 + def __le__(self, other): + return self.__cmp__(other) <= 0 -def main(): + def __ge__(self, other): + return self.__cmp__(other) >= 0 - maze = [[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, 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, 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, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]] + def __ne__(self, other): + return self.__cmp__(other) != 0 - start = (0, 0) - end = (7, 6) + def __repr__(self): + return str(self.cords) + ", g=" + str(self.g_cost) + ", h=" + str(self.h_cost) - path = astar(maze, start, end) - print(path) +class PointOutOfEnvironmentRangeException(Exception): + def __init__(self, message): + super(PointOutOfEnvironmentRangeException, self).__init__(message) diff --git a/src/environment/astar.py b/src/environment/astar.py deleted file mode 100644 index 5f877c0..0000000 --- a/src/environment/astar.py +++ /dev/null @@ -1,128 +0,0 @@ -from .line import Point - -import numpy - - -def a_star(environment, start, end): # (int[][], Point, Point) - visited = [] - non_visited = [] - start_node = Node(None, start) - non_visited.extend(get_neighbours(start_node, end)) - non_visited.sort() - - while len(non_visited) > 0: - lowest_cost_node = non_visited[0] - if lowest_cost_node.cords == end: - return reconstruct_path() - - non_visited.remove(lowest_cost_node) - visited.append(lowest_cost_node) - - non_visited.extend(get_neighbours(lowest_cost_node, end)) - # TODO have to add sth to not double some neighbours (with out it works but not sure) - non_visited.sort() - - return False - - -def reconstruct_path(): - return True - - -def already_visited(node, visited): # (Node, Node[]) - for visited_node in visited: - if node.cords == visited_node.cords: - return True - return False - - -def get_neighbours(node, end): # (Node, Point) - """ get all the node neighbours, without parent of the node""" - - neighbours_cords = [ - Point(-1, -1), Point(0, -1), Point(1, -1), - Point(-1, 0), Point(1, 0), - Point(-1, 1), Point(0, 1), Point(1, 1), - ] - - neighbours_nodes = [] - for cords in neighbours_cords: - - # if it is parent node we skip - if node.parent is not None and cords+node.cords == node.parent.cords: - continue - - # if we move vertically or horizontal we move 1, if diagonal sqrt(2) - diagonal_node = True - if cords.y == 0 or cords.x == 0: - diagonal_node = False - - cords += node.cords - - neighbour = Node(node, cords) - neighbour.g_cost = node.g_cost + (numpy.sqrt(2) if diagonal_node else 1) - neighbour.h_cost = diagonal_distance_heuristics(cords, end) - neighbours_nodes.append(neighbour) - - return neighbours_nodes - - -def diagonal_distance_heuristics(current, end): # (Point, Point) - """ we have to use different heuristic since we can move just in 8 direction, more info: - http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#diagonal-distance""" - - # d = 1 - d2 = numpy.sqrt(2) - dx = numpy.abs(end.x - current.x) - dy = numpy.abs(end.y - current.y) - - if dx > dy: - return (dx - dy) + d2 * dy - else: - return (dy - dx) + d2 * dx - - -# TODO change compare (bad code) -class Node: - def __init__(self, parent, cords): - self.parent = parent - self.cords = cords - self.g_cost = 0 # cost from start to current - self.h_cost = 0 # cost from current to end - - def f_cost(self): - return self.g_cost+self.h_cost - - def __cmp__(self, other): - if self.f_cost() > other.f_cost(): - return 1 - elif self.f_cost() < other.f_cost(): - return -1 - else: - if self.h_cost > other.h_cost: - return 1 - elif self.h_cost < other.h_cost: - return -1 - else: - return 0 - - def __lt__(self, other): - return self.__cmp__(other) < 0 - - def __gt__(self, other): - return self.__cmp__(other) > 0 - - def __eq__(self, other): - return self.__cmp__(other) == 0 - - def __le__(self, other): - return self.__cmp__(other) <= 0 - - def __ge__(self, other): - return self.__cmp__(other) >= 0 - - def __ne__(self, other): - return self.__cmp__(other) != 0 - - def __repr__(self): - return str(self.cords) + ", g=" + str(self.g_cost) + ", h=" + str(self.h_cost) diff --git a/src/environment/environment.py b/src/environment/environment.py index 57971a3..fb58aa2 100644 --- a/src/environment/environment.py +++ b/src/environment/environment.py @@ -1,11 +1,12 @@ import copy +import numpy from .environment_enum import Env -from .a_star import astar +from .a_star import a_star, diagonal_distance_heuristics from .line import Point, Line -def direction_map(environment, exit_points, step_size): +def direction_map(environment, exit_points, step_size): # (Point,Env)[][] / (int[][], Point[], int) """ Return direction map with adjusted step_size""" mapped_environment = copy.deepcopy(environment) for i in range(0, len(mapped_environment)): @@ -20,20 +21,37 @@ def direction_map(environment, exit_points, step_size): for y in range(0, len(environment)): for x in range(0, len(environment[y])): if mapped_environment[y][x] is None: - fastest_paths = [] + # TODO instead of counting a_star for each exit point we can choose best one with heuristic distance + # TODO I done it but its not best solution, + """ fastest_paths = [] """ + closest_point = exit_points[0] + shortest_distance = diagonal_distance_heuristics(Point(x, y), closest_point) for point in exit_points: - fastest_paths.append(astar(environment, (y, x), point)) - - shortest_path = fastest_paths[0] - distance_of_shortest_path = path_distance(shortest_path) - for path in fastest_paths: - if distance_of_shortest_path > path_distance(path): - shortest_path = path - distance_of_shortest_path = path_distance(path) + distance = diagonal_distance_heuristics(Point(x,y), point) + if distance < shortest_distance: + shortest_distance = distance + closest_point = point + # fastest_paths.append(a_star(environment, Point(x, y), point)) + + shortest_path = a_star(environment, Point(x, y), closest_point) + + """ we can skip this since we are getting closest point earlier """ + # distance_of_shortest_path = path_distance(shortest_path) + # for path in fastest_paths: + # if distance_of_shortest_path > path_distance(path): + # shortest_path = path + # distance_of_shortest_path = path_distance(path) + # TODO end of TODO for i in range(0, len(shortest_path)): - current_x = shortest_path[i][1] - current_y = shortest_path[i][0] + + current_x = shortest_path[i].x + current_y = shortest_path[i].y + + # check if it is pointless to continue mapping + if mapped_environment[current_y][current_x] is not None: + break + if i == len(shortest_path)-1: mapped_environment[current_y][current_x] = Env.EXIT else: @@ -41,12 +59,13 @@ def direction_map(environment, exit_points, step_size): possible_step_is_correct = True while possible_step >= 1: if i+possible_step < len(shortest_path): - point_to_go = Point(shortest_path[i + possible_step][1], shortest_path[i + possible_step][0]) + point_to_go = Point(shortest_path[i + possible_step].x, shortest_path[i + possible_step].y) else: last_index = len(shortest_path)-1 - point_to_go = Point(shortest_path[last_index][1], shortest_path[last_index][0]) + point_to_go = Point(shortest_path[last_index].x, shortest_path[last_index].y) possible_step = last_index - i + # TODO we can skipp it if step is 1 or 2 line = Line(Point(current_x, current_y), point_to_go) for line_obstacle in obstacles: if line.intersect(line_obstacle): @@ -56,15 +75,12 @@ def direction_map(environment, exit_points, step_size): else: possible_step_is_correct = True if possible_step_is_correct is True: - x_l = shortest_path[i + possible_step][1] - y_l = shortest_path[i + possible_step][0] - direction_point = Point(x_l, y_l) - mapped_environment[current_y][current_x] = direction_point + mapped_environment[current_y][current_x] = point_to_go break return mapped_environment -def get_obstacle_line_vertical(environment): +def get_obstacle_line_vertical(environment): # Line[] / int[][] is_line_started = False lines = [] for i in range(0, len(environment[0])): @@ -82,7 +98,7 @@ def get_obstacle_line_vertical(environment): return lines -def get_obstacle_line_horizon(environment): +def get_obstacle_line_horizon(environment): # Line[] / int[][] is_line_started = False lines = [] for i in range(0, len(environment)): @@ -100,50 +116,11 @@ def get_obstacle_line_horizon(environment): return lines -def map_environment(environment, exit_points): - """ Return mapped environment, each cord has next cord in 'fastest' path to exit (for step_size = 1)""" - """ its should be deleted because we have direciton map, but i will delete it in next push """ - mapped_environment = copy.deepcopy(environment) - for i in range(0, len(mapped_environment)): - for j in range(0, len(mapped_environment[i])): - if mapped_environment[i][j] == 1: - mapped_environment[i][j] = Env.OBSTACLE - else: - mapped_environment[i][j] = None - - for y in range(0, len(environment)): - for x in range(0, len(environment[y])): - if mapped_environment[y][x] is None: - fastest_paths = [] - print("befor astar") - for point in exit_points: - fastest_paths.append(astar(environment, (y, x), point)) - - print("after astar") - shortest_path = fastest_paths[0] - distance_of_shortest_path = path_distance(shortest_path) - for path in fastest_paths: - if distance_of_shortest_path > path_distance(path): - shortest_path = path - distance_of_shortest_path = path_distance(path) - - for i in range(0, len(shortest_path)): - current_x = shortest_path[i][1] - current_y = shortest_path[i][0] - if i == len(shortest_path)-1: - mapped_environment[current_y][current_x] = Env.EXIT - else: - mapped_environment[current_y][current_x] = shortest_path[i+1] - - return mapped_environment - - def path_distance(path): - distance = 0 for i in range(1, len(path)): - if path[i][1] != path[i-1][1] and path[i][0] != path[i-1][0]: - distance += 14 # approximation of sqrt(2) * 10 (*10 so it's integer(14) not float(1,4)) + if path[i].x != path[i-1].x and path[i].y != path[i-1].y: + distance += numpy.sqrt(2) else: - distance += 10 + distance += 1 return distance diff --git a/src/main.py b/src/main.py index 9a011d3..c73eeb8 100644 --- a/src/main.py +++ b/src/main.py @@ -1,35 +1,28 @@ import time -from environment.astar import diagonal_distance_heuristics, a_star -from environment.a_star import astar -from environment.environment import map_environment, get_obstacle_line_horizon, get_obstacle_line_vertical, \ - direction_map + +from environment.a_star import diagonal_distance_heuristics, a_star from environment.line import Point, Line -from environment.astar import Node, get_neighbours +from environment.environment import direction_map -import numpy maze = [[0, 0, 0, 0, 1, 0, 0, 0, 0, 0], + [0, 1, 1, 1, 1, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 1, 1, 1, 1, 0, 1], + [0, 0, 0, 0, 1, 0, 1, 1, 0, 1], + [0, 0, 0, 0, 1, 0, 1, 0, 0, 1], + [0, 0, 0, 0, 1, 0, 1, 1, 1, 1], [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, 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, 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, 0, 0, 0, 0, 0, 0]] + [0, 0, 0, 0, 1, 0, 1, 0, 1, 0], + [0, 0, 0, 0, 0, 0, 1, 0, 1, 0]] n=100 d = [[0 for j in range(0, n)] for i in range(0, n)] -for i in range(0, 25): - j = i - for j in range(0, j): - d[i][j] = 1 - -print(a_star(maze, Point(0, 0), Point(2,2))) +for i in range(0, 50): + d[i][50] = 1 # print(d) # @@ -50,33 +43,21 @@ # -# print(astar(d, (0, 0), (91,91))) -# -# for ele in direction_map(d, [(50, 1), (50, 2), (50, 3)], 3): +# for ele in direction_map(d, [Point(1, 9), Point(2, 9), Point(3, 9)], 1): # print(ele) +# print(a_star(d, Point(0,0), Point(99,99), d)) +# e = t2-t1 + -parent = Node(None, Point(0, 0)) -node = Node(parent, Point(1, 1)) -end = Point(3, 3) -node.g_cost = numpy.sqrt(2) -print(get_neighbours(node, end)) +#print(a_star(d, Point(0,0), Point(99,49))) -node.h_cost=1 +t1 = time.time() -nod2 = Node(None, Point(1, 1)) -nod2.g_cost = numpy.sqrt(2) -nod2.h_cost = 1 +for ele in direction_map(d, [Point(99, 49), Point(99, 48)], 2): + print(ele) +t2 = time.time() +print(str(t2 - t1)) -nod3 = Node(None, Point(1, 1)) -nod3.g_cost = 14 -nod3.h_cost = numpy.sqrt(2) -nod4 = Node(None, Point(1, 1)) -nod4.g_cost = 14 -nod4.h_cost = 2*numpy.sqrt(2) -l = [nod4, node, nod3, nod2] -print(l) -l.sort() -print(l)