-
Notifications
You must be signed in to change notification settings - Fork 20
/
gbfs.py
89 lines (76 loc) · 3.32 KB
/
gbfs.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
# Copyright 2019 Atikur Rahman Chitholian
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from queue import heappop, heappush
from math import inf
class Graph:
def __init__(self, directed=True):
self.edges = {}
self.huristics = {}
self.directed = directed
def add_edge(self, node1, node2, cost = 1, __reversed=False):
try: neighbors = self.edges[node1]
except KeyError: neighbors = {}
neighbors[node2] = cost
self.edges[node1] = neighbors
if not self.directed and not __reversed: self.add_edge(node2, node1, cost, True)
def set_huristics(self, huristics={}):
self.huristics = huristics
def neighbors(self, node):
try: return self.edges[node]
except KeyError: return []
def cost(self, node1, node2):
try: return self.edges[node1][node2]
except: return inf
def best_first_search(self, start, goal):
found, fringe, visited, came_from, cost_so_far = False, [(self.huristics[start], start)], set([start]), {start: None}, {start: 0}
print('{:11s} | {}'.format('Expand Node', 'Fringe'))
print('--------------------')
print('{:11s} | {}'.format('-', str(fringe[0])))
while not found and len(fringe):
_, current = heappop(fringe)
print('{:11s}'.format(current), end=' | ')
if current == goal: found = True; break
for node in self.neighbors(current):
new_cost = cost_so_far[current] + self.cost(current, node)
if node not in visited or cost_so_far[node] > new_cost:
visited.add(node); came_from[node] = current; cost_so_far[node] = new_cost
heappush(fringe, (new_cost + self.huristics[node], node))
print(', '.join([str(n) for n in fringe]))
if found: print(); return came_from, cost_so_far[goal]
else: print('No path from {} to {}'.format(start, goal)); return None, inf
@staticmethod
def print_path(came_from, goal):
parent = came_from[goal]
if parent:
Graph.print_path(came_from, parent)
else: print(goal, end='');return
print(' =>', goal, end='')
def __str__(self):
return str(self.edges)
graph = Graph(directed=True)
graph.add_edge('A', 'B', 4)
graph.add_edge('A', 'C', 1)
graph.add_edge('B', 'D', 3)
graph.add_edge('B', 'E', 8)
graph.add_edge('C', 'C', 0)
graph.add_edge('C', 'D', 7)
graph.add_edge('C', 'F', 6)
graph.add_edge('D', 'C', 2)
graph.add_edge('D', 'E', 4)
graph.add_edge('E', 'G', 2)
graph.add_edge('F', 'G', 8)
graph.set_huristics({'A': 8, 'B': 8, 'C': 6, 'D': 5, 'E': 1, 'F': 4, 'G': 0})
start, goal = 'A', 'G'
traced_path, cost = graph.best_first_search(start, goal)
if (traced_path): print('Path:', end=' '); Graph.print_path(traced_path, goal); print('\nCost:', cost)