126 lines
4.8 KiB
Python
126 lines
4.8 KiB
Python
#!/usr/bin/env python3
|
|
|
|
from enum import IntEnum, unique
|
|
from typing import List, Optional, Tuple, Dict
|
|
# IMPORTANT NOTE: DO NOT IMPORT THE ev3dev.ev3 MODULE IN THIS FILE
|
|
|
|
|
|
@unique
|
|
class Direction(IntEnum):
|
|
""" Directions in degrees """
|
|
NORTH = 0
|
|
EAST = 90
|
|
SOUTH = 180
|
|
WEST = 270
|
|
|
|
|
|
# simple alias, no magic here
|
|
Weight = int
|
|
"""
|
|
Weight of a given path (received from the server)
|
|
value: -1 if broken path
|
|
>0 for all other paths
|
|
never 0
|
|
"""
|
|
|
|
'''
|
|
https://www.python.org/dev/peps/pep-0289/
|
|
Node checking examples:
|
|
((0,0),Direction.NORTH) in planetmap[0]
|
|
Simplify this for node search:
|
|
next(x for (x, y), direction in planetmap[0])
|
|
next(y for x, y in planetmap[0] if x == (0, 0))
|
|
planetmap[0][next((x, y) for x, y in planetmap[0] if y == 'North')]
|
|
|
|
formatting example of bidirectional map:
|
|
{
|
|
(0, 0): {
|
|
<Direction.EAST: 90>: ((0, 0), <Direction.WEST: 270>, 1),
|
|
<Direction.WEST: 270>: ((0, 0), <Direction.EAST: 90>, 1),
|
|
<Direction.NORTH: 0>: ((0, 1), <Direction.SOUTH: 180>, 2)
|
|
},
|
|
(0, 1): {
|
|
<Direction.SOUTH: 180>: ((0, 0), <Direction.NORTH: 0>, 2)
|
|
}
|
|
}
|
|
'''
|
|
|
|
# Contains the representation of the map and provides certain functions to manipulate it according to the specifications
|
|
class Planet:
|
|
def __init__(self):
|
|
""" Initializes the data structure """
|
|
self._planetmap = {}
|
|
self.unexedge = []
|
|
self._curnode = None
|
|
self._curdir = Direction.NORTH
|
|
self.target = None
|
|
|
|
# Adds a bidirectional path defined between the start and end coordinates to the map and assigns the weight to it.
|
|
def add_path(self, start: Tuple[Tuple[int, int], Direction], target: Tuple[Tuple[int, int], Direction], weight: int):
|
|
if(start[0] not in self._planetmap):
|
|
self._planetmap[start[0]] = {}
|
|
if(start[1] not in self._planetmap[start[0]]):
|
|
self._planetmap[start[0]][start[1]] = (target[0], target[1], weight)
|
|
if(target[0] not in self._planetmap):
|
|
self._planetmap[target[0]] = {}
|
|
if(target[1] not in self._planetmap[target[0]]):
|
|
self._planetmap[target[0]][target[1]] = (start[0], start[1], weight)
|
|
|
|
def get_paths(self) -> Dict[Tuple[int, int], Dict[Direction, Tuple[Tuple[int, int], Direction, Weight]]]:
|
|
return self._planetmap
|
|
|
|
def setcurnode(self, node):
|
|
self._curnode = node
|
|
|
|
def getcurnode(self):
|
|
return self._curnode
|
|
|
|
def setcurdir(self, direction):
|
|
self._curdir = direction
|
|
|
|
def getcurdir(self):
|
|
return self._curdir
|
|
|
|
def checkedges(self, testnode):
|
|
validedges = []
|
|
if(self._curnode not in self._planetmap):
|
|
self._planetmap[self._curnode] = {}
|
|
for edges in testnode:
|
|
if(edges not in self._planetmap[self._curnode]):
|
|
validedges.append(edges)
|
|
if(validedges != [] and self._curnode not in self.unexedge):
|
|
self.unexedge.append(self._curnode)
|
|
if(len(validedges) <= 1 and self._curnode in self.unexedge):
|
|
self.unexedge.remove(self._curnode)
|
|
return validedges
|
|
|
|
'''
|
|
Returns a shortest path between two nodes.
|
|
Used Algorithm: Dijkstra's Algorithm
|
|
Have a look at heapqueue: https://docs.python.org/2/library/heapq.html
|
|
Formatting:
|
|
List = [(total_len, (current_node, [((node), Direction)]))]
|
|
'''
|
|
def shortest_path(self, start: Tuple[int, int], target: Tuple[int, int]) -> Optional[List[Tuple[Tuple[int, int], Direction]]]:
|
|
if((start in self._planetmap) and (target in self._planetmap)):
|
|
self.path_found = []
|
|
self.path_search = [(0, (start, []))]
|
|
while True:
|
|
for srcdir, (destnode, destdir, weight) in self._planetmap[self.path_search[0][1][0]].items():
|
|
if(destnode not in self.path_found and weight > 0): # check if in shortest_found
|
|
for x, (s_node, s_pathlist) in self.path_search:
|
|
if (destnode == s_node and self.path_search[0][0] + weight < x):
|
|
self.path_search.remove((x, (s_node, s_pathlist)))
|
|
if(s_weight for s_weight, (s_node, s_pathlist) in self.path_search if s_node == destnode):
|
|
self.path_search.append((self.path_search[0][0]+weight, (destnode, self.path_search[0][1][1] + [(self.path_search[0][1][0], srcdir)])))
|
|
self.path_found.append(self.path_search[0][1][0])
|
|
self.path_search.pop(0)
|
|
if(self.path_search == []): # if map is empty
|
|
print("Destination is not reachable from start.")
|
|
break
|
|
self.path_search.sort()
|
|
if(self.path_search[0][1][0] == target):
|
|
return self.path_search[0][1][1]
|
|
else:
|
|
print("Cannot calculate shortest Path. Start or target is not in known.")
|