def prepare(self): # TODO include in storing functions?
""" Computes a visibility graph optimized (=reduced) for path planning and stores it
Computes all directly reachable extremities based on visibility and their distance to each other
.. note::
Multiple polygon vertices might have identical coordinates.
They must be treated as distinct vertices here, since their attached edges determine visibility.
In the created graph however, these nodes must be merged at the end to avoid ambiguities!
.. note::
Pre computing the shortest paths between all directly reachable extremities
and storing them in the graph would not be an advantage, because then the graph is fully connected.
A star would visit every node in the graph at least once (-> disadvantage!).
"""
if self.prepared:
raise ValueError(
'this environment is already prepared. load new polygons first.'
)
# preprocessing the map
# construct graph of visible (=directly reachable) extremities
# and optimize graph further at construction time
# NOTE: initialise the graph with all extremities.
# even if a node has no edges (visibility to other extremities), it should still be included!
self.graph = DirectedHeuristicGraph(self.all_extremities)
extremities_to_check = self.all_extremities.copy()
# have to run for all (also last one!), because existing edges might get deleted every loop
while len(extremities_to_check) > 0:
# extremities are always visible to each other (bi-directional relation -> undirected graph)
# -> do not check extremities which have been checked already
# (would only give the same result when algorithms are correct)
# the extremity itself must not be checked when looking for visible neighbours
query_extremity: PolygonVertex = extremities_to_check.pop()
self.translate(new_origin=query_extremity)
visible_vertices = set()
candidate_extremities = extremities_to_check.copy()
# remove the extremities with the same coordinates as the query extremity
candidate_extremities.difference_update({
c
for c in candidate_extremities
if c.get_angle_representation() is None
})
# these vertices all belong to a polygon
n1, n2 = query_extremity.get_neighbours()
# ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
# eliminate all vertices 'behind' the query point from the candidate set
# since the query vertex is an extremity the 'outer' angle is < 180 degree
# then the difference between the angle representation of the two edges has to be < 2.0
# all vertices between the angle of the two neighbouring edges ('outer side')
# are not visible (no candidates!)
# ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
repr1 = n1.get_angle_representation()
repr2 = n2.get_angle_representation()
repr_diff = abs(repr1 - repr2)
candidate_extremities.difference_update(
find_within_range(repr1,
repr2,
repr_diff,
candidate_extremities,
angle_range_less_180=True,
equal_repr_allowed=False))
# as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
# such that both adjacent edges are visible, one will never visit e, because everything is
# reachable on a shorter path without e (except e itself).
# An extremity e1 lying in the area "in front of" extremity e hence is never the next vertex
# in a shortest path coming from e.
# And also in reverse: when coming from e1 everything else than e itself can be reached faster
# without visiting e.
# -> e1 and e do not have to be connected in the graph.
# IMPORTANT: this condition only holds for building the basic visibility graph without start and goal node!
# When a query point (start/goal) happens to be an extremity, edges to the (visible) extremities in front
# MUST be added to the graph!
# Find extremities which fulfill this condition for the given query extremity
repr1 = (repr1 + 2.0) % 4.0 # rotate 180 deg
repr2 = (repr2 + 2.0) % 4.0
# IMPORTANT: the true angle diff does not change, but the repr diff does! compute again
repr_diff = abs(repr1 - repr2)
# IMPORTANT: check all extremities here, not just current candidates
# do not check extremities with equal coordinates (also query extremity itself!)
# and with the same angle representation (those edges must not get deleted from graph!)
temp_candidates = set(
filter(lambda e: e.get_angle_representation() is not None,
self.all_extremities))
lie_in_front = find_within_range(repr1,
repr2,
repr_diff,
temp_candidates,
angle_range_less_180=True,
equal_repr_allowed=False)
# "thin out" the graph -> optimisation
# already existing edges in the graph to the extremities in front have to be removed
self.graph.remove_multiple_undirected_edges(
query_extremity, lie_in_front)
# do not consider when looking for visible extremities (NOTE: they might actually be visible!)
candidate_extremities.difference_update(lie_in_front)
# all edges except the neighbouring edges (handled above!) have to be checked
edges_to_check = set(self.all_edges)
edges_to_check.remove(query_extremity.edge1)
edges_to_check.remove(query_extremity.edge2)
visible_vertices.update(
find_visible(candidate_extremities, edges_to_check))
self.graph.add_multiple_undirected_edges(query_extremity,
visible_vertices)
self.graph.make_clean() # join all nodes with the same coordinates
self.prepared = True
def find_shortest_path(
self,
start_coordinates: INPUT_COORD_TYPE,
goal_coordinates: INPUT_COORD_TYPE,
free_space_after: bool = True,
verify: bool = True) -> Tuple[PATH_TYPE, LENGTH_TYPE]:
""" computes the shortest path and its length between start and goal node
:param start_coordinates: a (x,y) coordinate tuple representing the start node
:param goal_coordinates: a (x,y) coordinate tuple representing the goal node
:param free_space_after: whether the created temporary search graph self.temp_graph
should be deleted after the query
:param verify: whether it should be checked if start and goal points really lie inside the environment.
if points close to or on polygon edges should be accepted as valid input, set this to ``False``.
:return: a tuple of shortest path and its length. ([], None) if there is no possible path.
"""
# path planning query:
# make sure the map has been loaded and prepared
if self.boundary_polygon is None:
raise ValueError('No Polygons have been loaded into the map yet.')
if not self.prepared:
self.prepare()
if verify and not self.within_map(start_coordinates):
raise ValueError('start point does not lie within the map')
if verify and not self.within_map(goal_coordinates):
raise ValueError('goal point does not lie within the map')
if start_coordinates == goal_coordinates:
# start and goal are identical and can be reached instantly
return [start_coordinates, goal_coordinates], 0.0
# could check if start and goal nodes have identical coordinates with one of the vertices
# optimisations for visibility test can be made in this case:
# for extremities the visibility has already been (except for in front) computed
# BUT: too many cases possible: e.g. multiple vertices identical to query point...
# -> always create new query vertices
# include start and goal vertices in the graph
start_vertex = Vertex(start_coordinates)
goal_vertex = Vertex(goal_coordinates)
# check the goal node first (earlier termination possible)
self.translate(new_origin=goal_vertex
) # do before checking angle representations!
# IMPORTANT: manually translate the start vertex, because it is not part of any polygon
# and hence does not get translated automatically
start_vertex.mark_outdated()
# the visibility of only the graphs nodes has to be checked (not all extremities!)
# points with the same angle representation should not be considered visible
# (they also cause errors in the algorithms, because their angle repr is not defined!)
candidates = set(
filter(lambda n: n.get_angle_representation() is not None,
self.graph.get_all_nodes()))
# IMPORTANT: check if the start node is visible from the goal node!
# NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
candidates.add(start_vertex)
visibles_n_distances_goal = find_visible(candidates,
edges_to_check=set(
self.all_edges))
if len(visibles_n_distances_goal) == 0:
# The goal node does not have any neighbours. Hence there is not possible path to the goal.
return [], None
# create temporary graph TODO make more performant, avoid real copy
# DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph
# but to still not create real copies of vertex instances!
self.temp_graph = deepcopy(self.graph)
# IMPORTANT geometrical property of this problem: it is always shortest to directly reach a node
# instead of visiting other nodes first (there is never an advantage through reduced edge weight)
# -> when goal is directly reachable, there can be no other shorter path to it. Terminate
for v, d in visibles_n_distances_goal:
if v == start_vertex:
return [start_coordinates, goal_coordinates], d
# add unidirectional edges to the temporary graph
# add edges in the direction: extremity (v) -> goal
self.temp_graph.add_directed_edge(v, goal_vertex, d)
self.translate(new_origin=start_vertex
) # do before checking angle representations!
# the visibility of only the graphs nodes have to be checked
# the goal node does not have to be considered, because of the earlier check
candidates = set(
filter(lambda n: n.get_angle_representation() is not None,
self.graph.get_all_nodes()))
visibles_n_distances_start = find_visible(candidates,
edges_to_check=set(
self.all_edges))
if len(visibles_n_distances_start) == 0:
# The start node does not have any neighbours. Hence there is not possible path to the goal.
return [], None
# add edges in the direction: start -> extremity
self.temp_graph.add_multiple_directed_edges(
start_vertex, visibles_n_distances_start)
# also here unnecessary edges in the graph can be deleted when start or goal lie in front of visible extremities
# IMPORTANT: when a query point happens to coincide with an extremity, edges to the (visible) extremities
# in front MUST be added to the graph! Handled by always introducing new (non extremity, non polygon) vertices.
# for every extremity that is visible from either goal or start
# NOTE: edges are undirected! self.temp_graph.get_neighbours_of(start_vertex) == set()
# neighbours_start = self.temp_graph.get_neighbours_of(start_vertex)
neighbours_start = {n for n, d in visibles_n_distances_start}
# the goal vertex might be marked visible, it is not an extremity -> skip
neighbours_start.discard(goal_vertex)
neighbours_goal = self.temp_graph.get_neighbours_of(goal_vertex)
for vertex in neighbours_start | neighbours_goal:
# assert type(vertex) == PolygonVertex and vertex.is_extremity
# check only if point is visible
temp_candidates = set()
if vertex in neighbours_start:
temp_candidates.add(start_vertex)
if vertex in neighbours_goal:
temp_candidates.add(goal_vertex)
if len(temp_candidates) > 0:
self.translate(new_origin=vertex)
# IMPORTANT: manually translate the goal and start vertices
start_vertex.mark_outdated()
goal_vertex.mark_outdated()
n1, n2 = vertex.get_neighbours()
repr1 = (n1.get_angle_representation() +
2.0) % 4.0 # rotated 180 deg
repr2 = (n2.get_angle_representation() + 2.0) % 4.0
repr_diff = abs(repr1 - repr2)
# IMPORTANT: special case:
# here the nodes must stay connected if they have the same angle representation!
lie_in_front = find_within_range(repr1,
repr2,
repr_diff,
temp_candidates,
angle_range_less_180=True,
equal_repr_allowed=False)
self.temp_graph.remove_multiple_undirected_edges(
vertex, lie_in_front)
# NOTE: exploiting property 2 from [1] here would be more expensive than beneficial
vertex_path, distance = self.temp_graph.modified_a_star(
start_vertex, goal_vertex)
if free_space_after:
del self.temp_graph # free the memory
# extract the coordinates from the path
return [tuple(v.coordinates) for v in vertex_path], distance
def drawFOV(guardOrPad,
rooms,
tiles,
guards,
objects,
opaque_objects,
plt,
ignoreTileAddrs=None,
objTransforms=None):
"""
[!] The rooms you give must be simply linked i.e. project flat. i.e. only 1 floor
Otherwise this will likely enter an infinite loop looking for the boundary
Nothing is drawn inside the guard's current room
NOTE: If a ray glances off an object and out of our room it will currently cause us big problems
"""
if objTransforms is None:
objTransforms = dict()
guardPos = guardOrPad["position"]
if len(guardPos) == 3:
guardPos = guardPos[::2]
guardRoom = tiles[guardOrPad["tile"]]["room"]
# 0. Ignore tiles used to ensure our boundary polygon doesn't overlap at all
ignoreTileAddrs = set() if ignoreTileAddrs is None else set(
ignoreTileAddrs)
# 1. get tiles in our rooms (or use all tiles)
envTileAddrs = set(tiles.keys() if rooms is None else
[a for a, t in tiles.items() if t["room"] in rooms])
envTileAddrs.difference_update(ignoreTileAddrs)
assert len(envTileAddrs) > 0
externalEdges = set()
# 2. Find the outside shape:
# Get a point with max x (and z). Then from those tiles with this point,
# get one with a maximal 2nd point
max_p = max(p for a in envTileAddrs for p in tiles[a]["points"])
maxTiles = [a for a in envTileAddrs if max_p in tiles[a]["points"]]
q, addr = max(
(p, a) for a in maxTiles for p in tiles[a]["points"] if p != max_p)
pnts = tiles[addr]["points"]
i = pnts.index(q)
if pnts[i - 1] != max_p:
i = (i + 1) % len(pnts)
assert pnts[i] == max_p
# 3a/4a. Get all external edges.
# Prepare to get clipping context, which will map to (loop, index)
# Where loop == -1 means outerBoundary, otherwise holes[loop]
remExtEdges = set([(addr, ((i + 1) % len(tiles[addr]["links"])) - 1)
for addr in envTileAddrs
for i, l in enumerate(tiles[addr]["links"])
if l == 0 or l not in envTileAddrs])
clippingContext = dict()
objectContext = dict()
# Now [i-1], [i] are these two points, and must be an external edge.
# (note this is only external to the group we're looking at, but that's okay)
# And this edge is ["links"][i-1]. And they go ACWS
assert (addr, i - 1) in remExtEdges
outerBoundary, outerOrigins = walkClippingBoundary(addr, i - 1,
envTileAddrs, tiles,
remExtEdges)
clippingContext.update(
dict(zip(outerBoundary, zip(repeat(-1), count(), outerOrigins))))
# 3. Find all clipping holes. Untested.
# Also we'll pick up any extra
holes = []
while len(remExtEdges) > 0:
addr, i = next(iter(remExtEdges))
hole, holeOrigins = walkClippingBoundary(addr, i, envTileAddrs, tiles,
remExtEdges)
holes.append(hole) # CWS
clippingContext.update(
dict(zip(hole, zip(repeat(len(holes) - 1), count(), holeOrigins))))
# 4. Get objs
cleanObjPnts = dict()
for room in rooms:
for objAddr in opaque_objects[room]:
pnts = objects[objAddr]["points"]
# Skip doors unless we're transforming them
if objects[objAddr][
"type"] == "door" and objAddr not in objTransforms:
continue
dists = [
np.linalg.norm(np.subtract(p, q))
for p, q in zip(pnts, pnts[1:] + pnts[:1])
]
pnts = [p for p, d in zip(pnts, dists) if d > 0.05]
if len(pnts) <= 2: # glass
continue
if objAddr in objTransforms:
pnts = [tuple(objTransforms[objAddr](p)) for p in pnts]
cleanObjPnts[objAddr] = pnts
objectContext.update(dict(zip(pnts, zip(repeat(objAddr),
count()))))
holes.append(pnts[::-1])
# 5. Use library, digging a little into the internals to add the current path
environment = PolygonEnvironment()
environment.store(
outerBoundary[::-1], holes,
validate=True) # probably don't validate O:) - objects may leak over
environment.prepare()
# Poking internals working okay..
assert environment.within_map(guardPos)
guardVertex = Vertex(guardPos)
environment.translate(new_origin=guardVertex)
candidates = set(
filter(lambda n: n.get_angle_representation() is not None,
environment.graph.get_all_nodes()))
visibles = [
tuple(pnt[0].coordinates)
for pnt in find_visible(candidates,
edges_to_check=set(environment.all_edges))
]
# 6. Extend these points as far as possible.
# Arrange visibles in a (A?)CWS order
# Probably use more than just the coordinates - see if it's brushing the corner or going into it.
# Lack of another point between them says they land on the same edge - save some processing
# May even be able to infer if we're brushing past?
# But ultimately we are doing Rare code - generalise that code which we walked down the frig stairs with,
# then test for intersection with each object.
# May need to reach back to get the tiles from the points
# Also for final drawing restrict it to the room which Nat aint in (pass as param, can generalise to tiles if needed later).
visibles.sort(key=lambda v: atan2(*np.subtract(v, guardPos)))
inside = True
borderData = None
currPoly = None
far = False
for p in visibles:
# Get the next and previous points
# Note that the orientations are opposite for the outerBoundary
# .. though maybe not coz of the way we store them
assert (p in clippingContext) ^ (p in objectContext)
pnts = j = loopI = origins = None
isClipping = p in clippingContext
if isClipping:
loopI, j, (tileAddr, tilePntI) = clippingContext[p]
pnts = outerBoundary if loopI == -1 else holes[loopI]
else:
objAddr, j = objectContext[p]
pnts = cleanObjPnts[objAddr]
assert p == pnts[j]
a = pnts[j - 1]
b = pnts[(j + 1) % len(pnts)]
# Determine if we glance or crash into this vertex
# Get the 2 edges, both should be CWS around the shape, then turned in
v = rotCWS(np.subtract(a, p))
w = rotCWS(np.subtract(p, b))
ray = np.subtract(p, guardPos)
glances = not ((np.dot(v, ray) > 0) and (np.dot(w, ray) > 0))
n = rotACWS(ray)
n = np.multiply(n, 1 / np.linalg.norm(n))
a = np.dot(n, guardPos)
if not glances:
q = p
if isClipping:
lastTile = tiles[tileAddr]
else:
_, lastTile, _ = walkAcrossTiles(guardOrPad["tile"],
n,
a,
envTileAddrs, [0],
tiles,
endPoint=p)
if isinstance(lastTile, int):
lastTile = tiles[lastTile]
else:
# Awkward case of glancing clipping corner
# If we started at the source, we touch the clipping so could wrongly stop
# We still need to fetch all the tiles to this point.
visitedTiles = []
if isClipping:
# Rotate around the corner until our ray leaves through an edge (rather than a corner)
# We are pretty sure this just means rotating CWS,
# because this is against the direction we set up the clipping
while True:
assert tiles[tileAddr]["points"][tilePntI] == p
q = tiles[tileAddr]["points"][tilePntI - 1]
l = tiles[tileAddr]["links"][tilePntI - 1]
v2 = rotCWS(np.subtract(q, p)) # p -> q, rotated
if np.dot(v2, ray) < 0:
break
tilePntI = tiles[l]["links"].index(tileAddr)
tileAddr = l
_ = walkAcrossTiles(guardOrPad["tile"],
n,
a,
envTileAddrs, [0],
tiles,
endPoint=p,
visitedTiles=visitedTiles)
else:
# For an object, we can't easily establish a start tile,
# so we just start at the source
tileAddr = guardOrPad["tile"]
# Get the far clipping collision, and complete the list of tiles
# There may be a duplicate but this isn't an issue.
_, lastTile, q = walkAcrossTiles(tileAddr,
n,
a,
envTileAddrs, [0],
tiles,
visitedTiles=visitedTiles)
p, q = map(tuple, (p, q))
lastInGuardRoom = len(visitedTiles) - 1 - [
tiles[a]["room"] for a in visitedTiles[::-1]
].index(guardRoom)
# Search for collisions with objects and clipping holes
# This is proper line Segment intersection
for pnts in holes:
prevPnt = pnts[-1]
for i, pnt in enumerate(pnts):
q = getLineSegmentIntersection(prevPnt, pnt, p, q, n, a, q)
prevPnt = pnt
if lastTile["room"] != guardRoom:
vt = (visitedTiles +
[lastTile])[lastInGuardRoom:lastInGuardRoom +
2] # hacky patch, sometimes he's not included
assert len(vt) == 2
borderData = (vt, n, a, p, q)
if inside:
# Exiting
currPoly = [borderData, []]
inside = False
if glances and far:
currPoly[1].append(q)
currPoly[1].append(p)
if glances and not far:
currPoly[1].append(q)
if glances:
far = not far
else:
if not inside:
# Entering
currPoly.append(borderData)
# currPoly = (entryData, points, leaveData)
# Find the points where we enter and leave
borderPnts = []
for (insideTile, outsideTile), n, a, p, q in currPoly[0:3:2]:
# Tiles may only share a point if we had to walk around, in which case the point is p
if outsideTile in tiles[insideTile]["links"]:
i = tiles[insideTile]["links"].index(outsideTile)
d = tiles[insideTile]["points"][i + 1]
c = tiles[insideTile]["points"][i]
borderPnts.append(
getLineSegmentIntersection(c, d, p, q, n, a, None,
True))
else:
borderPnts.append(p)
# Drop first and last point in favour of these border points. Wrap.
polyPnts = borderPnts[:1] + currPoly[1][1:-1] + borderPnts[
1:] + borderPnts[:1]
xs, zs = zip(*polyPnts)
xs = [-x for x in xs]
plt.fill(xs, zs, linewidth=1, fc='r', alpha=0.2)
inside = True