def TakeAction(self, action):
if not self.injail:
proposal = Point((self.current_state.x + action[0],
self.current_state.y + action[1]))
#print proposal.x, proposal.y
path = LineString([self.current_state, proposal])
if self.obstacles:
for obstacle in self.obstacles:
if obstacle.intersects(path):
self.injail = True
self.current_state = Point((-1, -1))
return
if self.muds:
for mud in self.muds:
if mud.intersects(path):
# print 'we are here'
path_inmud = mud.intersection(path)
coords = [path.coords[0], path.coords[1]]
for loc in path_inmud.coords:
if loc not in coords:
coords.append(loc)
coords.sort(key=lambda tup: tup[1])
p_in_mud = proposal.intersects(mud)
s_in_mud = self.current_state.intersects(mud)
if p_in_mud and not s_in_mud:
# print 'current not in mud'
if coords.index((self.current_state.x,
self.current_state.y)) == 0:
x = coords[1][0] - coords[0][0] + 0.5 * (
coords[-1][0] - coords[1][0])
y = coords[1][1] - coords[0][1] + 0.5 * (
coords[-1][1] - coords[1][1])
proposal = Point(
(coords[0][0] + x, coords[0][1] + y))
else:
x = coords[1][0] - coords[-1][0] + 0.5 * (
coords[0][0] - coords[1][0])
y = coords[1][1] - coords[-1][1] + 0.5 * (
coords[0][1] - coords[1][1])
proposal = Point(
(coords[-1][0] + x, coords[-1][1] + y))
elif s_in_mud and not p_in_mud:
# print 'proposal not in mud'
if coords.index((self.current_state.x,
self.current_state.y)) == 0:
x = 0.5 * (coords[1][0] - coords[0][0]) + (
coords[-1][0] - coords[1][0])
y = 0.5 * (coords[1][1] - coords[0][1]) + (
coords[-1][1] - coords[1][1])
proposal = Point(
(coords[0][0] + x, coords[0][1] + y))
else:
x = 0.5 * (coords[1][0] - coords[-1][0]) + (
coords[0][0] - coords[1][0])
y = 0.5 * (coords[1][1] - coords[-1][1]) + (
coords[0][1] - coords[1][1])
proposal = Point(
(coords[-1][0] + x, coords[-1][1] + y))
else:
proposal = Point(
(self.current_state.x + action[0] * 0.5,
self.current_state.y + action[1] * 0.5))
path = LineString([self.current_state, proposal])
bounds = LinearRing(self.maze.exterior.coords)
if bounds.intersects(path):
onedge = bounds.intersection(path)
if type(onedge) is MultiPoint:
for point in onedge:
if not point.equals(self.current_state):
proposal = point
elif type(onedge) is Point:
if not onedge.equals(self.current_state):
proposal = onedge
elif not self.maze.contains(proposal):
proposal = bounds.interpolate(bounds.project(proposal))
self.current_state = proposal
else:
self.deadend_toleration = self.deadend_toleration - 1
return self.GetCurrentState()
def intersectionsGridFrontiere(P, X, Y):
"""Calcule la trace de la grille cartésienne définie par X, Y sur le Polyligne P
autrement dit les intersections de P avec les droites
- x = X[i] verticales et
- y = Y[j] horizontales
qui représentent la grille.
:param P : un polyligne np.ndarray de shape (n,3) ou (n,2). La 3-eme dimension est ignorée.
:param X, Y : la grille cartésienne.
X et Y sont des np.ndarray de shape (nx,1) et (ny,1) qui représentent
les abscisses et les ordonnées de la grille
- On suppose que X et Y sont croissants (i)
- On suppose également que la grille recouvre entièrement P, et déborde, i.e.
min(X) < xmin(P) <= xmax(P) < max(X)
min(Y) < ymin(P) <= ymax(P) < max(Y)
:return PD: np.ndarray((npd,2)) contenant les points
"""
# nx, ny = len(X), len(Y)
# for x in X : plt.plot(ny*[x], Y, 'y-', linewidth=0.3,)
# for y in Y : plt.plot(X, nx*[y], 'y-', linewidth=0.3)
# plt.plot(P[:,0],P[:,1], 'r.')
# P = LineString(P)
P = LinearRing(P)
# x,y = P.xy
# plt.plot(x,y,'g-')
# plt.axes().set_aspect('equal')
# debug(P)
(minx, miny, maxx, maxy) = P.bounds
# debug(P.bounds)
#Les numeros de droites verticales intersectant P
iX = np.where(np.logical_and(minx < X, X < maxx))[0]
# px = X[iX]#une croix pour marquer les droite concernées (graphique)
# plt.plot(px, len(px)*[min(Y)], 'rx')
#les droites verticales
ms = [((X[i], Y[0]), (X[i], Y[-1])) for i in iX]
#Les numeros de droites horizontales intersectant P
iY = np.where(np.logical_and(miny < Y, Y < maxy))[0]
# px = Y[iY]#une croix pour marquer les droite concernées
# plt.plot(len(px)*[max(X)], px, 'rx')
# plt.legend()
# plt.show()
#Les droites horizontales concernées par P
ms.extend([((X[0], Y[i]), (X[-1], Y[i])) for i in iY])
D = MultiLineString(ms) #La famille des droites de la grille
# debug(len(D))
#convex_hull pour réordonner les points, en faire un polygone
#array_interface pour recuperer les data pures numpy
PD = P.intersection(D) #.array_interface()
# debug(type(PD))
D = [P.project(pd)
for pd in PD] #abscisse curviligne des points d'intersection
D.sort()
PD = [P.interpolate(d).xy for d in D] #Les points dans l'ordre
# PD = PD.array_interface()
# exit()
#.convex_hull.exterior
# shape = PD['shape']
#PD.reshape(...) : si le tableau PD doit être recopié => en silence
#PD = array(PD['data']).reshape(shape)
# PD = array(PD['data'])
PD = array(PD)
#PD.shape=... : si le tableau PD doit être recopié => erreur
# PD.shape = shape
return PD