Implemented pathfinding for 2 node paths and multiple paths

1 files changed, 50 insertions, 47 deletions

diff --git a/main.py b/main.py
index e1bee18..c204d91 100644
--- a/main.py
+++ b/main.py
@@ -1,66 +1,69 @@
-import numpy as np
-
-# x---x---x---x---x
-# |           |   |
-# | A         |   |
-# |           |   |
-# x   x---x   x---x
-# |               |
-# |               |
-# |               |
-# x---x       x   x
-# |   |       |   |
-# S   |       | C |
-# |   |       |   |
-# x   x       x   x
-# |               |
-# |               |
-# |               |
-# x   x---x   x---x
-# |       |       |
-# |     B |     D |
-# |     L |       |
-# x---x---x---x---x
-
 mapWidth = 4
 mapHeight = 5
-currentPos = [2, 2]
+startPos = [0, 2]
+endPos = [1, 4]
+
+checkpointList = [
+        startPos,
+        [0, 0],
+        [1, 4],
+        [3, 2],
+        [3, 4],
+        endPos
+        ]
+
+obstacleList = [
+        [[2, 0], [3, 0]], # obstacleList[0]
+        [[1, 0], [1, 1]], # obstacleList[1]
+        [[3, 0], [3, 1]],
+        [[0, 1], [0, 2]],
+        [[0, 2], [1, 2]],
+        [[2, 2], [3, 2]],
+        [[1, 3], [1, 4]],
+        [[3, 3], [3, 4]],
+        [[1, 4], [2, 4]]
+        ]
 
-allNodes = []
+courseMap = []
 for x in range(mapWidth): # Like for(int x = 0; x > 20; i++) {
     for y in range(mapHeight):
-        allNodes.append([x, y])
+        courseMap.append([x, y])
 
-def queryNeighbors(node, obstacleList):
+def queryNeighbors(node):
 #                 Right    Down    Left     Up
     directions = [[1,0], [0, 1], [-1, 0], [0, -1]]
     result = []
     for dir in directions: # Basically for x in range(4)
         neighbor = [node[0] + dir[0], node[1] + dir[1]] # [nodeX + DirectionModifierX, nodeY + DirectionModifierY]
-        if neighbor in allNodes: # If neighbor is present in the grid:
+        if neighbor in courseMap: # If neighbor is present in the grid:
             result.append(neighbor)
             for x in range(len(obstacleList)):
                 if node in obstacleList[x] and neighbor in obstacleList[x]:
                     result.remove(neighbor)
     return result
 
-obstacleList = [
-        [[2, 0], [3, 0]], # obstacleList[0]
-        [[1, 0], [1, 1]], # obstacleList[1]
-        [[3, 0], [3, 1]],
-        [[0, 1], [0, 2]],
-        [[0, 2], [1, 2]],
-        [[2, 2], [3, 2]],
-        [[1, 3], [1, 4]],
-        [[3, 3], [3, 4]],
-        [[1, 4], [2, 4]]
-        ]
+def getShortestPath(courseMap, startPos, endPos):
+    searchPaths = [[startPos]]
+    visitedCoordinates = [startPos]
+
+    while searchPaths != []:
+        currentPath = searchPaths.pop(0)
+        currentPos = currentPath[-1]
+
+        if currentPos == endPos:
+            return currentPath
+
+        for nextPos in queryNeighbors(currentPos):
+            if nextPos in visitedCoordinates:
+                continue
+            searchPaths.append(currentPath + [nextPos])
+            visitedCoordinates += nextPos
+
+def solveCourse(courseMap, obstacleList, checkpointList):
+    for x in range(len(checkpointList) - 1): # Repeat 6 Times
+        shortestPath = getShortestPath(courseMap, checkpointList[x], checkpointList[x + 1])
+        print(shortestPath)
+    
+solveCourse(courseMap, obstacleList, checkpointList)
 
-# print('OBSTACLES:')
-# for x in range(len(obstacleList)):
-#     print(obstacleList[x])
-print('CURRENT POSITION:')
-print(currentPos)
-print('NEIGHBORS:')
-print(queryNeighbors(currentPos, obstacleList))