def get_final_course(self, lastIndex):
path = [[self.goal.x, self.goal.y]]
while self.node_list[lastIndex].parent is not None:
node = self.node_list[lastIndex]
path.append([node.x, node.y])
lastIndex = node.parent
path.append([self.start.x, self.start.y])
return path
def CreatePath(start_point, finish_point, obst_vect):
'''Return list of Path points(tuples)'''
Path = [] # list of path points (tuples)
Path.append(start_point) # beginning
i = 0 # auxiliary variable
Path.append(CalculateNextPoint(Path, finish_point,obst_vect))
while (Path[-1] != finish_point):
NextPoint = CalculateNextPoint(Path, finish_point,obst_vect)
if NextPoint == Path[-2]: # Robot might alternate between 2 positions
# make a new move in a random direction
Rand_X = np.random.choice([-delta, 0, delta])
Rand_Y = np.random.choice([-delta, 0, delta])
NewDir = (Rand_X, Rand_Y)
RandomPoint = tuple(map(lambda x, y: x + y, NextPoint, NewDir))
Path.append(RandomPoint)
else:
Path.append(NextPoint)
i += 1
if i > PathLength: # Path points limit
print("Couldn't finish the Path in %s iterations :(" % PathLength)
break
# assert(i < PathLength), "Couldn't finish the Path in 100000 iterations :("
return Path
def shortestPath(G,start,end):
"""
Find a single shortest path from the given start vertex
to the given end vertex.
The input has the same conventions as Dijkstra().
The output is a list of the vertices in order along
the shortest path.
"""
D,P = Dijkstra(G,start,end)
Path = []
while 1:
Path.append(end)
if end == start: break
end = P[end]
Path.reverse()
return Path
lw = 25.0
verts = np.array([[-0.1, -0.5],
[ 0.0, +0.5],
[+0.1, -0.5]])
verts = verts*200 + (400,400)
codes = [Path.MOVETO, Path.LINETO,Path.LINETO ]
path = Path(verts, codes)
patch = patches.PathPatch(path, facecolor='none', lw=lw, alpha=.25)
patch.set_path_effects([PathEffects.Stroke(capstyle='butt')])
axes.add_patch(patch)
path = PathCollection()
path.append(verts)
w = lw/2.0
P = []
for vertex in path[0].vertices:
position = vertex['a_position']
segment = vertex['a_segment']
t1 = vertex['a_tangents'][:2]
t1 /= np.sqrt(((t1*t1)).sum())
t2 = vertex['a_tangents'][2:]
t2 /= np.sqrt(((t2*t2)).sum())
u,v = vertex['a_texcoord']
angle = np.arctan2 (t1[0]*t2[1]-t1[1]*t2[0], t1[0]*t2[0]+t1[1]*t2[1])
t = t1+t2
t /= np.sqrt(((t*t)).sum())
o = np.array([t[1], -t[0]])
if event.key == 'escape': sys.exit()
fig.canvas.mpl_connect('key_press_event', on_key)
lw = 25.0
verts = np.array([[-0.1, -0.5], [0.0, +0.5], [+0.1, -0.5]])
verts = verts * 200 + (400, 400)
codes = [Path.MOVETO, Path.LINETO, Path.LINETO]
path = Path(verts, codes)
patch = patches.PathPatch(path, facecolor='none', lw=lw, alpha=.25)
patch.set_path_effects([PathEffects.Stroke(capstyle='butt')])
axes.add_patch(patch)
path = PathCollection()
path.append(verts)
w = lw / 2.0
P = []
for vertex in path[0].vertices:
position = vertex['a_position']
segment = vertex['a_segment']
t1 = vertex['a_tangents'][:2]
t1 /= np.sqrt(((t1 * t1)).sum())
t2 = vertex['a_tangents'][2:]
t2 /= np.sqrt(((t2 * t2)).sum())
u, v = vertex['a_texcoord']
angle = np.arctan2(t1[0] * t2[1] - t1[1] * t2[0],
t1[0] * t2[0] + t1[1] * t2[1])
t = t1 + t2
t /= np.sqrt(((t * t)).sum())
