Merge pull request #136 from KRHero03/master

Added A Star Algorithm for Path Finder

pygamelib/actuators.py

@@ -23,6 +23,7 @@
 from pygamelib import constants
 import random
 import collections
+from queue import PriorityQueue
 
 
 class Actuator:
@@ -331,11 +332,18 @@ class PathFinder(Behavioral):
         method is going to circle between the waypoints
         (when the last is visited, go back to the first)
     :type circle_waypoints: bool
+    :param algorithm: ALGO_BFS - BFS, ALGO_ASTAR - AStar
+    :type algorithm: constant
 
     """
 
     def __init__(
-        self, game=None, actuated_object=None, circle_waypoints=True, parent=None
+        self,
+        game=None,
+        actuated_object=None,
+        circle_waypoints=True,
+        parent=None,
+        algorithm=constants.ALGO_BFS,
     ):
         if actuated_object is not None and parent is None:
             self.actuated_object = actuated_object
@@ -350,6 +358,15 @@ def __init__(
         self.waypoints = []
         self._waypoint_index = 0
         self.circle_waypoints = circle_waypoints
+        self.algorithm = algorithm
+        if type(self.algorithm) is not int or (
+            self.algorithm != constants.ALGO_BFS
+            and self.algorithm != constants.ALGO_ASTAR
+        ):
+            raise base.PglInvalidTypeException(
+                "In Actuator.PathFinder.__init__(..,algorithm) algorithm must be"
+                "either ALGO_BFS or ALGO_ASTAR."
+            )
 
     def set_destination(self, row=0, column=0):
         """Set the targeted destination.
@@ -377,9 +394,7 @@ def find_path(self):
         """Find a path to the destination.
 
         Destination (PathFinder.destination) has to be set beforehand.
-        This method implements a Breadth First Search algorithm
-        (`Wikipedia <https://en.wikipedia.org/wiki/Breadth-first_search>`_)
-        to find the shortest path to destination.
+
 
         Example::
 
@@ -394,6 +409,18 @@ def find_path(self):
         .. warning:: PathFinder.destination is a tuple!
             Please use PathFinder.set_destination(x,y) to avoid problems.
 
+        Path Finding Algorithm Description:
+
+        Breadth First Search:
+         This method implements a Breadth First Search algorithm
+        (`Wikipedia <https://en.wikipedia.org/wiki/Breadth-first_search>`_)
+        to find the shortest path to destination.
+
+        A* Search:
+        This method implements a A* Search algorithm
+        (`Wikipedia <https://en.wikipedia.org/wiki/A*_search_algorithm>`_)
+        to find the shortest path to destination.
+
         """
         if self.actuated_object is None:
             raise base.PglException(
@@ -411,7 +438,12 @@ def find_path(self):
                 "destination is not defined",
                 "PathFinder.destination has to be defined.",
             )
+        if self.algorithm == constants.ALGO_BFS:
+            return self.__find_path_bfs()
+
+        return self.__find_path_astar()
 
+    def __find_path_bfs(self):
         queue = collections.deque(
             [[(self.actuated_object.pos[0], self.actuated_object.pos[1])]]
         )
@@ -436,6 +468,51 @@ def find_path(self):
                     seen.add((r, c))
         return []
 
+    def __find_path_astar(self):
+
+        queue = PriorityQueue()
+
+        # queue stores a tuple with values:
+        # h - heuristic value = depth + manhattan distance from current node to destination
+        # type(h) = int
+        # path - path to reach current node from start node
+        # type(path) = list
+        # For each node, depth = len(path)
+
+        initial_h = abs(self.actuated_object.pos[0] - self.destination[0]) + abs(
+            self.actuated_object.pos[1] - self.destination[1]
+        )
+
+        queue.put(
+            (initial_h, [(self.actuated_object.pos[0], self.actuated_object.pos[1])])
+        )
+        seen = set()
+        while not queue.empty():
+            h_val, path = queue.get()
+            x, y = path[-1]
+            if (x, y) == self.destination:
+                self._current_path = path
+                # We return only a copy of the path as we need to keep the
+                # real one untouched for our own needs.
+                return path.copy()
+
+            # r = row c = column
+            for r, c in ((x + 1, y), (x - 1, y), (x, y + 1), (x, y - 1)):
+                h_val = (
+                    len(path)
+                    + abs(self.destination[0] - r)
+                    + abs(self.destination[1] - c)
+                )
+                if (
+                    0 <= c < self.game.current_board().size[0]
+                    and 0 <= r < self.game.current_board().size[1]
+                    and self.game.current_board().item(r, c).overlappable()
+                    and ((r, c) not in seen)
+                ):
+                    queue.put((h_val, path + [(r, c)]))
+                    seen.add((r, c))
+        return []
+
     def current_path(self):
         """This method simply return a copy of the current path of the actuator.
 
@@ -673,7 +750,7 @@ def next_waypoint(self):
         return self.waypoints[self._waypoint_index]
 
     def remove_waypoint(self, row, column):
-        """ Remove a waypoint from the stack.
+        """Remove a waypoint from the stack.
 
         This method removes the first occurrence of a waypoint in the stack.
 

pygamelib/constants.py

@@ -81,3 +81,7 @@
 NO_PLAYER = 90000001
 MODE_RT = 90000002
 MODE_TBT = 90000003
+
+# Path Finding Algorithm Constants
+ALGO_BFS = 90000100
+ALGO_ASTAR = 90000101

tests/test_actuators.py

@@ -68,7 +68,7 @@ def test_unidirectional(self):
         a = actuators.UnidirectionalActuator(direction=None)
         self.assertEqual(a.next_move(), constants.RIGHT)
 
-    def test_pathfinder(self):
+    def test_pathfinder_bfs(self):
         npc = board_items.NPC()
         b = engine.Board()
         g = engine.Game()
@@ -78,6 +78,13 @@ def test_pathfinder(self):
         g.change_level(1)
         actuators.PathFinder(actuated_object=npc)
         npc.actuator = actuators.PathFinder(parent=npc, game=g, circle_waypoints=False)
+        with self.assertRaises(engine.base.PglInvalidTypeException):
+            actuators.PathFinder(
+                    parent=npc, 
+                    game=g,
+                    circle_waypoints=False,
+                    algorithm="constants.ALGO_BFS"
+                )
         npc.actuator.set_destination(2, 2)
         npc.actuator.find_path()
         self.assertTrue(len(npc.actuator.current_path()) > 0)
@@ -88,6 +95,91 @@ def test_pathfinder(self):
             npc.actuator.find_path()
             self.assertEqual(e.error, "actuated_object is not defined")
         npc.actuator.actuated_object = board_items.Door()
+        with self.assertRaises(engine.base.PglException) as e:
+            npc.actuator.find_path()
+            self.assertEqual(e.error, "actuated_object not a Movable object")
+        npc.actuator.actuated_object = board_items.Door()
+        npc.actuator.actuated_object = npc
+        npc.actuator.destination = None
+        with self.assertRaises(engine.base.PglException) as e:
+            npc.actuator.find_path()
+            self.assertEqual(e.error, "destination is not defined")
+        b.place_item(board_items.Wall(), 2, 2)
+        npc.actuator.set_destination(2, 2)
+        self.assertEqual(npc.actuator.find_path(), [])
+        # These tests are a recipe of how to NOT do things...
+        npc.actuator.destination = (None, None)
+        self.assertEqual(npc.actuator.next_move(), constants.NO_DIR)
+        npc.actuator.set_destination(5, 5)
+        npc.actuator._current_path = []
+        npc.actuator.next_move()
+        npc.actuator.set_destination(2, 5)
+        npc.actuator._current_path = []
+        nm = npc.actuator.next_move()
+        self.assertEqual(nm, constants.UP)
+        npc.actuator.add_waypoint(5, 6)
+        npc.actuator.add_waypoint(6, 6)
+        npc.actuator.add_waypoint(5, 4)
+        npc.actuator.add_waypoint(4, 6)
+        nm = None
+        while nm != constants.NO_DIR:
+            nm = npc.actuator.next_move()
+            b.move(npc, nm, npc.step)
+        with self.assertRaises(engine.base.PglInvalidTypeException):
+            npc.actuator.add_waypoint(5, "6")
+        with self.assertRaises(engine.base.PglInvalidTypeException):
+            npc.actuator.add_waypoint("5", 6)
+        npc.actuator.clear_waypoints()
+        self.assertEqual(npc.actuator.next_waypoint(), (None, None))
+        npc.actuator.clear_waypoints()
+        npc.actuator.destination = (None, None)
+        npc.actuator.add_waypoint(10, 10)
+        npc.actuator.add_waypoint(12, 15)
+        self.assertEqual(npc.actuator.destination, (10, 10))
+        self.assertEqual(npc.actuator.next_waypoint(), (10, 10))
+        self.assertEqual(npc.actuator.next_waypoint(), (12, 15))
+        self.assertEqual(npc.actuator.next_waypoint(), (None, None))
+        npc.actuator.circle_waypoints = True
+        self.assertEqual(npc.actuator.next_waypoint(), (10, 10))
+        with self.assertRaises(engine.base.PglInvalidTypeException):
+            npc.actuator.remove_waypoint(10, "10")
+        with self.assertRaises(engine.base.PglInvalidTypeException):
+            npc.actuator.remove_waypoint("10", 10)
+        with self.assertRaises(engine.base.PglException) as e:
+            npc.actuator.remove_waypoint(30, 30)
+            self.assertEqual(e.error, "invalid_waypoint")
+        self.assertIsNone(npc.actuator.remove_waypoint(10, 10))
+
+    def test_pathfinder_astar(self):
+        npc = board_items.NPC()
+        b = engine.Board()
+        g = engine.Game()
+        g.player = board_items.Player()
+        g.add_board(1, b)
+        g.add_npc(1, npc, 5, 5)
+        g.change_level(1)
+        actuators.PathFinder(actuated_object=npc)
+        npc.actuator = actuators.PathFinder(
+            parent=npc, game=g, circle_waypoints=False, algorithm=constants.ALGO_ASTAR
+        )
+        with self.assertRaises(engine.base.PglInvalidTypeException):
+            actuators.PathFinder(
+                    parent=npc, 
+                    game=g,
+                    circle_waypoints=False,
+                    algorithm="constants.ALGO_ASTAR"
+                )
+        npc.actuator.set_destination(2, 2)
+        npc.actuator.find_path()
+        self.assertTrue(len(npc.actuator.current_path()) > 0)
+
+        with self.assertRaises(engine.base.PglInvalidTypeException):
+            npc.actuator.set_destination("2", 2)
+        npc.actuator.actuated_object = None
+        with self.assertRaises(engine.base.PglException) as e:
+            npc.actuator.find_path()
+            self.assertEqual(e.error, "actuated_object is not defined")        
+        npc.actuator.actuated_object = board_items.Door()
         with self.assertRaises(engine.base.PglException) as e:
             npc.actuator.find_path()
             self.assertEqual(e.error, "actuated_object not a Movable object")