my a* algorithm almost done #6

src/__init__.py

@@ -1 +0,0 @@
-from environment import *

src/environment/astar.py

@@ -0,0 +1,128 @@
+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)

src/environment/environment.py

@@ -1,8 +1,8 @@
 import copy
 
-from environment.environment_enum import Env
-from environment.a_star import astar
-from environment.line import Point, Line
+from .environment_enum import Env
+from .a_star import astar
+from .line import Point, Line
 
 
 def direction_map(environment, exit_points, step_size):

src/environment/line.py

@@ -66,5 +66,11 @@ def __init__(self, x, y):
         self.x = x
         self.y = y
 
+    def __add__(self, other):
+        return Point(self.x+other.x, self.y+other.y)
+
     def __repr__(self):
         return "(" + str(self.x) + ", " + str(self.y) + ")"
+
+    def __eq__(self, other):
+        return self.x == other.x and self.y == other.y

src/main.py

@@ -1,7 +1,13 @@
+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.line import Point, Line
+from environment.astar import Node, get_neighbours
+
+import numpy
 
 maze = [[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
         [0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
@@ -14,20 +20,63 @@
         [0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
         [0, 0, 0, 0, 0, 0, 0, 0, 0, 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)))
+
+# print(d)
+#
+#
+# print(get_obstacle_line_horizon(maze))
+# print(get_obstacle_line_vertical(maze))
+#
+# p1 = Point(0, 0)
+# p2 = Point(6, 0)
+#
+# p3 = Point(4, 0)
+#
+# l1 = Line(p1, p2)
+# l2 = Line(p3, p3)
+
+# for ele in map_environment(maze, [(9, 1)]):
+#     print(ele)
+
+
+#
+# print(astar(d, (0, 0), (91,91)))
+#
+# for ele in direction_map(d, [(50, 1), (50, 2), (50, 3)], 3):
+#     print(ele)
+
+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(get_obstacle_line_horizon(maze))
-print(get_obstacle_line_vertical(maze))
+node.h_cost=1
 
-p1 = Point(0, 0)
-p2 = Point(6, 0)
+nod2 = Node(None, Point(1, 1))
+nod2.g_cost = numpy.sqrt(2)
+nod2.h_cost = 1
 
-p3 = Point(4, 0)
+nod3 = Node(None, Point(1, 1))
+nod3.g_cost = 14
+nod3.h_cost = numpy.sqrt(2)
 
-l1 = Line(p1, p2)
-l2 = Line(p3, p3)
+nod4 = Node(None, Point(1, 1))
+nod4.g_cost = 14
+nod4.h_cost = 2*numpy.sqrt(2)
 
-for ele in map_environment(maze, [(9, 1)]):
-    print(ele)
+l = [nod4, node, nod3, nod2]
+print(l)
 
-for ele in direction_map(maze, [(9, 1)], 3):
-    print(ele)
+l.sort()
+print(l)