def __init__(self, graph, path=None): self.root = tk.Tk() self.root.geometry("1200x800") self.root.config(bg='white') self.canvas = tk.Canvas(width=1200, height=800) self.canvas.config(bg='white') self.radius = 15 for node in graph.nodes: pos = node.position self.canvas.create_oval(pos[0] - self.radius, pos[1] - self.radius, pos[0] + self.radius, pos[1] + self.radius, fill='red') self.canvas.create_text(pos[0], pos[1], text=str(node.id), anchor='center') for succ in node.successors: to_succ = VecMath.normal(VecMath.sub(succ.position, node.position)) arrow_start = VecMath.add(node.position, VecMath.multiply(to_succ, self.radius)) arrow_end = VecMath.sub(succ.position, VecMath.multiply(to_succ, self.radius)) self.canvas.create_line(arrow_start[0], arrow_start[1], arrow_end[0], arrow_end[1], arrow=tk.LAST) if path is not None: self.path_lines = [] for i in range(1, len(path)): a_to_b = VecMath.normal(VecMath.sub(path[i].position, path[i - 1].position)) arrow_start = VecMath.add(path[i - 1].position, VecMath.multiply(a_to_b, self.radius)) arrow_end = VecMath.sub(path[i].position, VecMath.multiply(a_to_b, self.radius)) self.path_lines.append( self.canvas.create_line(arrow_start[0], arrow_start[1], arrow_end[0], arrow_end[1], arrow=tk.LAST, dash=(2, 2), fill='yellow', width=10)) self.canvas.update() self.canvas['bg'] = 'green' self.canvas.pack() self.root.mainloop()
def cost_func(path): """ This cost function just returns the linear distance between the nodes (assumed to be spatial nodes)""" cost = 0 for i in range(1, len(path)): cost += VecMath.length( VecMath.sub(path[i - 1].position, path[i].position)) return cost
def linear_distance_heuristic(path, goal): return VecMath.length(VecMath.sub(goal.position, path[-1].position))