Added basic pathfinding: need to implement obstacles/shuffling etc

figure3.txt

@@ -0,0 +1,11 @@
+8
+45
+SSS.SSSSSSSSSSSSSSS...SSSSSSSSSSSSSSSSSSSSS..
+SSS.SSSSSSSSSSSSSSS...SSSSSSSSSSSSSSSSSSSSS..
+.............................................
+SSS..SSSSSSSSSSSSSS...SSSSSSSSSSSSSSSSSSSSS..
+SSS..SSSSSSSSSSSSSS...SSSSSSSSSSSSSSSSSSSSS..
+SSS..SSSSSSSSSSSSSS...SSSSSSSSSSSSSSSSSSSSS..
+.............................................
+SSS.SSSSSSSSSSSSSSS...SSSSSSSSSSSSSSSSSSSSS..
+SSS.SSSSSSSSSSSSSSS...SSSSSSSSSSSSSSSSSSSSS..

main.py

@@ -2,9 +2,11 @@
 #(height, width)
 import random as rnd
 import os
+import itertools
 from enum import Enum
 import time
 import math
+from queue import PriorityQueue as pq
 
 DEBUG=True
 
@@ -19,6 +21,9 @@ width=0
 grid=[]
 # entries=[(3,0)]
 entries=[(0,3)]
+# entries=[(7,3),(7,43)]
+# entries=[(7,3)]
+
 
 class GridSquare:
     #typ: SquareType, occupant: index
@@ -51,25 +56,48 @@ def mooreNeighbourhood(pos):
             i[0] >= 0 and i[1] >= 0 and i[0] < height and i[1] < width]
 
 def nextSquare(passenger, grid):
-    #simple behaviour for now
-    candidati = []
+    dist=[[0 for x in range(0,width)] for y in range(0,height)]
+
     dest = None
+
     if passenger.inter == None:
         dest = passenger.dest
+        print("dest")
     else:
         dest = passenger.inter
-    for i in mooreNeighbourhood(passenger.curr):
-        if manhattanDistance(dest, i) < manhattanDistance(dest, passenger.curr):
-            # if grid[passenger.curr[0]][passenger.curr[1]].typ == SquareType.SEAT and i[1] != passenger.curr[1] and False:
-            if grid[passenger.curr[0]][passenger.curr[1]].typ == SquareType.SEAT and i[1] != passenger.curr[1]:
-                pass
+
+    q = pq()
+
+    for y in range(0,height):
+        for x in range(0,width):
+            if x != passenger.curr[1] or y != passenger.curr[0]:
+                dist[y][x] = 10**10
+
+    mn = mooreNeighbourhood(passenger.curr)
+    for (i, man) in enumerate(mn):
+        q.put((1, man, i))
+        dist[man[0]][man[1]] = 1
+
+    while not q.empty():
+        u = q.get()
+        if u[1] == dest:
+            return mn[u[2]]
+        for x in mooreNeighbourhood(u[1]):
+            alt = 0
+            if grid[x[0]][x[1]].typ == SquareType.SEAT:
+                alt = dist[u[1][0]][u[1][1]] + 5
             else:
-                candidati.append(i)
-    for i in candidati:
-        if grid[i[0]][i[1]].typ == SquareType.AISLE and grid[i[0]][i[1]].occupant != None: #this should work well enough for one aisle plane
-            return i
-    #todo: implement shufflin'
-    return candidati[0]
+                alt = dist[u[1][0]][u[1][1]] + 1
+            # if alt == 0:
+            # if x == dest:
+            #     print("mn: " + str(x) + ", alt: " + str(alt))
+            #     print("dist" + str(x) + ": " + str(dist[x[0]][x[1]]))
+            if alt < dist[x[0]][x[1]]:
+                dist[x[0]][x[1]] = alt
+                q.put((alt, x, u[2]))
+
+    return passenger.curr
+
 
 def GenPassList():
     global height
@@ -98,8 +126,9 @@ def GenPassList():
                     grid[y][x].typ = SquareType.AISLE
     
     for x in border:
+        sign=(-1)**rnd.randint(0,1)
         for i in range(1,7):
-            inc = (-1)**i * -math.floor((i+1)/2)
+            inc = (-1)**i * sign * math.floor((i+1)/2)
             if (inc + x.dest[0] > 0) and (inc + x.dest[0] < height) and (grid[x.dest[0]+inc][x.dest[1]].typ == SquareType.AISLE):
                 x.inter=(inc + x.dest[0], x.dest[1])
 
@@ -112,7 +141,7 @@ passengers = GenPassList()
 # passengers.reverse()
 rnd.shuffle(passengers)
 toad = 0
-btime = 3 #passengers board every btime ticks
+btime = 10 #passengers board every btime ticks
 
 
 
@@ -126,6 +155,15 @@ def tick(t):
     global btime
     # print grid
     if DEBUG:
+        for x in range(0, width):
+            if x % 10 == 0:
+                print(chr(0xFF10 + int(x / 10)), end="")
+            else:
+                print(chr(0x3000), end="")
+        print()
+        for x in range(0, width):
+            print(chr(0xFF10 + (x % 10)), end="")
+        print()
         for row in grid:
             for guy in row:
                 if guy.occupant is not None:
@@ -155,7 +193,8 @@ def tick(t):
             continue
         # if i == 36 and DEBUG:
         #     print(nextSquare(man, grid))
-        print(str(man.curr) + ", " + str(man.inter) + ", " + str(man.dest))
+        if DEBUG:
+            print(str(man.curr) + ", " + str(man.inter) + ", " + str(man.dest))
         done=False
         #im sexist
         nextS=nextSquare(man,grid)
@@ -172,7 +211,7 @@ def tick(t):
         if man.shuffled != 0:
             man.shuffled-=1
     if DEBUG:
-        time.sleep(0.01)
+        time.sleep(0.05)
         # time.sleep(0.05)
         os.system("clear")
     if done and toad >= len(passengers):