2025-summer/team-3
12
Fork 0
Code Issues Pull requests Projects Releases Activity

start implementing routing by creating new router

Browse source
This commit is contained in:
Laura Klünder 2017-11-27 02:16:20 +01:00
parent 587b08a1e4
commit dd3a885749
14 changed files with 219 additions and 1735 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/c3nav/mapdata/models/geometry/level.py
View file

@ -162,7 +162,7 @@ class AltitudeArea(LevelGeometryMixin, models.Model):
slope = np.array(self.point2) - np.array(self.point1)
distances = (np.sum(((points - np.array(self.point1)) * slope), axis=1) / (slope ** 2).sum()).clip(0, 1)
return self.altitude + distances*(self.altitude2-self.altitude)
return float(self.altitude) + distances*(float(self.altitude2)-float(self.altitude))
@classmethod
def recalculate(cls):

127
src/c3nav/routing/area.py
View file

@ -1,127 +0,0 @@
from itertools import combinations
import numpy as np
from matplotlib.path import Path
from c3nav.routing.utils.coords import coord_angle
class GraphArea():
def __init__(self, room, mpl_clear, mpl_stairs, escalators, points=None):
self.room = room
self.graph = room.graph
self.mpl_clear = mpl_clear
self.mpl_stairs = mpl_stairs
self.escalators = escalators
self.points = points
def serialize(self):
return (
self.mpl_clear,
self.mpl_stairs,
self.escalators,
self.points,
)
def prepare_build(self):
self._built_points = []
def build_connections(self):
for point1, point2 in combinations(self._built_points, 2):
there, back, distance = self.check_connection(point1.xy, point2.xy)
if there is not None:
point1.connect_to(point2, distance=distance, ctype=there)
if back is not None:
point2.connect_to(point1, distance=distance, ctype=back)
def check_connection(self, point1, point2):
path = Path(np.vstack((point1, point2)))
distance = abs(np.linalg.norm(point1 - point2))
# lies within room
if self.mpl_clear.intersects_path(path):
return None, None, None
if self.room.stuffedareas.intersects_path(path, filled=True):
distance *= 2.5
# stair checker
angle = coord_angle(point1, point2)
stair_direction_up = None
for stair_path, stair_angle in self.mpl_stairs:
if not path.intersects_path(stair_path):
continue
angle_diff = ((stair_angle - angle + 180) % 360) - 180
new_direction_up = (angle_diff > 0)
if stair_direction_up is None:
stair_direction_up = new_direction_up
elif stair_direction_up != new_direction_up:
return None, None, None
if not (40 < abs(angle_diff) < 150):
return None, None, None
# escalator checker
angle = coord_angle(point1, point2)
escalator_direction_up = None
escalator_swap_direction = False
for escalator in self.escalators:
if not escalator.mpl_geom.intersects_path(path, filled=True):
continue
if escalator_direction_up is not None:
# only one escalator per connection
return None, None, None
angle_diff = ((escalator.angle - angle + 180) % 360) - 180
escalator_direction_up = (angle_diff > 0)
escalator_swap_direction = (escalator_direction_up != escalator.direction_up)
if stair_direction_up is not None:
return (
('stairs_up' if stair_direction_up else 'stairs_down'),
('stairs_down' if stair_direction_up else 'stairs_up'),
distance,
)
elif escalator_direction_up is not None:
if not escalator_swap_direction:
return ('escalator_up' if escalator_direction_up else 'escalator_down'), None, distance
else:
return None, ('escalator_down' if escalator_direction_up else 'escalator_up'), distance
return '', '', distance
def add_point(self, point):
if not self.mpl_clear.contains_point(point.xy):
return False
self._built_points.append(point)
return True
def finish_build(self):
self.points = np.array(tuple(point.i for point in self._built_points))
set_points = set(self.points)
if len(self.points) != len(set_points):
print('ERROR: POINTS DOUBLE-ADDED (AREA)', len(self.points), len(set_points))
def contains_point(self, point):
return self.mpl_clear.contains_point(point)
def connected_points(self, point, mode):
connections = {}
for point_i in self.points:
other_point = self.graph.points[point_i]
there, back, distance = self.check_connection(point, other_point.xy)
ctype = there if mode == 'orig' else back
if ctype is not None:
connections[point_i] = (distance, ctype)
return connections

20
src/c3nav/routing/connection.py
View file

@ -1,20 +0,0 @@
import numpy as np
from django.utils.functional import cached_property
from c3nav.routing.utils.coords import coord_angle
class GraphConnection():
def __init__(self, from_point, to_point, distance=None, ctype=''):
self.from_point = from_point
self.to_point = to_point
self.distance = distance if distance is not None else abs(np.linalg.norm(from_point.xy - to_point.xy))
self.ctype = ctype
@cached_property
def angle(self):
return coord_angle(self.from_point.xy, self.to_point.xy)
def __repr__(self):
return ('<GraphConnection %r %r distance=%f ctype=%s>' %
(self.from_point, self.to_point, self.distance, self.ctype))

543
src/c3nav/routing/graph.py
View file

@ -1,543 +0,0 @@
# flake8: noqa
import os
import pickle
from collections import OrderedDict, namedtuple
from itertools import combinations
import numpy as np
from django.conf import settings
from scipy.sparse.csgraph._shortest_path import shortest_path
from scipy.sparse.csgraph._tools import csgraph_from_dense
from c3nav.mapdata.models.level import Level
from c3nav.mapdata.models.locations import Location, LocationGroup
from c3nav.routing.connection import GraphConnection
from c3nav.routing.exceptions import AlreadyThere, NoRouteFound, NotYetRoutable
from c3nav.routing.level import GraphLevel
from c3nav.routing.point import GraphPoint
from c3nav.routing.route import NoRoute, Route
from c3nav.routing.routesegments import (GraphRouteSegment, LevelRouteSegment, RoomRouteSegment, SegmentRoute,
SegmentRouteWrapper)
class Graph:
graph_cached = None
graph_cached_mtime = None
default_filename = os.path.join(settings.DATA_DIR, 'graph.pickle')
def __init__(self, mtime=None):
self.mtime = mtime
self.levels = OrderedDict()
for level in Level.objects.all():
self.levels[level.name] = GraphLevel(self, level)
self.points = []
self.level_transfer_points = None
self.elevatorlevel_points = None
# Building the Graph
def build(self):
self._built_level_transfer_points = []
self._built_levelconnector_points = {}
self._built_elevatorlevel_points = {}
for level in self.levels.values():
level.build()
# collect rooms and points
rooms = sum((level.rooms for level in self.levels.values()), [])
self.points = sum((level._built_points for level in self.levels.values()), [])
# create connections between levels
print()
self.connect_levelconnectors()
self.connect_elevators()
# finishing build: creating numpy arrays and convert everything else to tuples
self.points = tuple(set(self.points))
for i, room in enumerate(rooms):
room.i = i
for i, point in enumerate(self.points):
point.i = i
self.level_transfer_points = tuple(point.i for point in self._built_level_transfer_points)
for level in self.levels.values():
level.finish_build()
print()
print('Total:')
self.print_stats()
print()
print('Points per room:')
for name, level in self.levels.items():
print(('Level %s:' % name), *(sorted((len(room.points) for room in level.rooms), reverse=True)))
def print_stats(self):
print('%d points' % len(self.points))
print('%d rooms' % sum(len(level.rooms) for level in self.levels.values()))
print('%d level transfer points' % len(self.level_transfer_points))
print('%d connections' % sum(level.connection_count() for level in self.levels.values()))
def add_levelconnector_point(self, levelconnector, point):
self._built_levelconnector_points.setdefault(levelconnector.name, []).append(point)
def add_elevatorlevel_point(self, elevatorlevel, point):
self._built_elevatorlevel_points[elevatorlevel.name] = point
def connect_levelconnectors(self):
from c3nav.mapdata.models.geometry import LevelConnector
for levelconnector in LevelConnector.objects.all():
center = levelconnector.geometry.centroid
points = self._built_levelconnector_points.get(levelconnector.name, [])
rooms = set(point.room for point in points if point.room is not None)
connected_levels = set(room.level for room in rooms)
should_levels = tuple(level.name for level in levelconnector.levels.all())
missing_levels = set(should_levels) - set(level.level.name for level in connected_levels)
if missing_levels:
print('levelconnector %s on levels %s at (%.2f, %.2f) is not connected to levels %s!' %
(levelconnector.name, ', '.join(should_levels), center.x, center.y, ', '.join(missing_levels)))
continue
center_point = GraphPoint(center.x, center.y, None)
self.points.append(center_point)
self._built_level_transfer_points.append(center_point)
for level in connected_levels:
level._built_room_transfer_points.append(center_point)
level._built_points.append(center_point)
for room in rooms:
room._built_points.append(center_point)
for point in points:
center_point.connect_to(point)
point.connect_to(center_point)
def connect_elevators(self):
for elevator in Elevator.objects.all(): # noqa
elevatorlevels = tuple(elevator.elevatorlevels.all())
for level1, level2 in combinations(elevatorlevels, 2):
point1 = self._built_elevatorlevel_points[level1.name]
point2 = self._built_elevatorlevel_points[level2.name]
center = GraphPoint((point1.x+point2.x)/2, (point1.y+point2.y)/2, None)
self.points.append(center)
self._built_level_transfer_points.append(center)
for room in (point1.room, point2.room):
room._built_points.append(center)
room.level._built_room_transfer_points.append(center)
room.level._built_points.append(center)
direction_up = level2.altitude > level1.altitude
dist = abs(level2.altitude-level1.altitude)
point1.connect_to(center, ctype=('elevator_up' if direction_up else 'elevator_down'), distance=dist)
center.connect_to(point2, ctype=('elevator_up' if direction_up else 'elevator_down'), distance=dist)
point2.connect_to(center, ctype=('elevator_down' if direction_up else 'elevator_up'), distance=dist)
center.connect_to(point1, ctype=('elevator_down' if direction_up else 'elevator_up'), distance=dist)
# Loading/Saving the Graph
def serialize(self):
return (
{name: level.serialize() for name, level in self.levels.items()},
[point.serialize() for point in self.points],
self.level_transfer_points,
)
def save(self, filename=None):
if filename is None:
filename = self.default_filename
with open(filename, 'wb') as f:
pickle.dump(self.serialize(), f)
@classmethod
def unserialize(cls, data, mtime):
levels, points, level_transfer_points = data
graph = cls(mtime=mtime)
for name, level in levels.items():
graph.levels[name].unserialize(level)
rooms = sum((level.rooms for level in graph.levels.values()), ())
graph.points = tuple(GraphPoint(x, y, None if room is None else rooms[room]) for x, y, room in points)
graph.level_transfer_points = level_transfer_points
for i, room in enumerate(rooms):
room.i = i
for i, point in enumerate(graph.points):
point.i = i
return graph
@classmethod
def load(cls, filename=None):
do_cache = False
if filename is None:
do_cache = True
filename = cls.default_filename
graph_mtime = None
if do_cache:
graph_mtime = os.path.getmtime(filename)
if cls.graph_cached is not None:
if cls.graph_cached_mtime == graph_mtime:
return cls.graph_cached
with open(filename, 'rb') as f:
graph = cls.unserialize(pickle.load(f), graph_mtime)
if do_cache:
cls.graph_cached_mtime = graph_mtime
cls.graph_cached = graph
graph.print_stats()
return graph
# Drawing
def draw_pngs(self, points=True, lines=True):
for level in self.levels.values():
level.draw_png(points, lines)
# Router
def build_routers(self, allowed_ctypes, allow_nonpublic, avoid, include):
routers = {}
empty_distances = np.empty(shape=(len(self.level_transfer_points),) * 2, dtype=np.float16)
empty_distances[:] = np.inf
sparse_distances = empty_distances.copy()
level_transfers = np.zeros(shape=(len(self.level_transfer_points),) * 2, dtype=np.int16)
level_transfers[:] = -1
for i, level in enumerate(self.levels.values()):
routers.update(level.build_routers(allowed_ctypes, allow_nonpublic, avoid, include))
router = routers[level]
level_distances = empty_distances.copy()
in_level_i = np.array(tuple(level.room_transfer_points.index(point)
for point in level.level_transfer_points), dtype=int)
in_graph_i = np.array(tuple(self.level_transfer_points.index(point)
for point in level.level_transfer_points), dtype=int)
level_distances[in_graph_i[:, None], in_graph_i] = router.shortest_paths[in_level_i[:, None], in_level_i]
better = level_distances < sparse_distances
sparse_distances[better] = level_distances[better]
level_transfers[better] = i
g_sparse = csgraph_from_dense(sparse_distances, null_value=np.inf)
shortest_paths, predecessors = shortest_path(g_sparse, return_predecessors=True)
routers[self] = GraphRouter(shortest_paths, predecessors, level_transfers)
return routers
def get_location_points(self, location: Location, mode):
if isinstance(location, PointLocation):
points = self.levels[location.section.name].connected_points(np.array((location.x, location.y)), mode)
if not points:
return (), None, None
points, distances, ctypes = zip(*((point, distance, ctype) for point, (distance, ctype) in points.items()))
distances = {points[i]: distance for i, distance in enumerate(distances)}
points = np.array(points, dtype=np.int)
return points, distances, ctypes
try:
if isinstance(location, AreaLocation):
points = self.levels[location.level.name].arealocation_points[location.name]
return points, None, None
elif isinstance(location, LocationGroup):
points = tuple(np.hstack(tuple(self.get_location_points(area, mode)[0]
for area in location.arealocations.all())).astype(np.int))
return points, None, None
except KeyError:
raise NotYetRoutable
def _get_points_by_i(self, points):
return tuple(self.points[i] for i in points)
def _allowed_points_index(self, points, allowed_points_i):
return np.array(tuple(i for i, point in enumerate(points) if point in allowed_points_i))
def get_route(self, origin: Location, destination: Location,
allowed_ctypes, allow_nonpublic, avoid, include, visible_nonpublic_areas=None):
orig_points_i, orig_distances, orig_ctypes = self.get_location_points(origin, 'orig')
dest_points_i, dest_distances, dest_ctypes = self.get_location_points(destination, 'dest')
if not len(orig_points_i) or not len(dest_points_i):
raise NoRouteFound()
if orig_distances is None and dest_distances is None:
if set(dest_points_i) & set(orig_points_i):
orig_points_i = tuple(set(orig_points_i) - set(dest_points_i))
if set(dest_points_i) & set(orig_points_i):
dest_points_i = tuple(set(dest_points_i) - set(orig_points_i))
if not len(orig_points_i) or not len(dest_points_i):
raise AlreadyThere()
# if set(orig_points_i) & set(dest_points_i):
# raise AlreadyThere()
add_orig_point = origin if isinstance(origin, PointLocation) else None
add_dest_point = destination if isinstance(destination, PointLocation) else None
orig_points = self._get_points_by_i(orig_points_i)
dest_points = self._get_points_by_i(dest_points_i)
common_points = tuple(set(orig_points) & set(dest_points))
best_route = NoRoute
# get routers
routers = self.build_routers(allowed_ctypes, allow_nonpublic, avoid, include)
# route within room
orig_rooms = set(point.room for point in orig_points)
dest_rooms = set(point.room for point in dest_points)
common_rooms = orig_rooms & dest_rooms
# rooms are directly connectable
if add_orig_point and add_dest_point and common_rooms:
room = tuple(common_rooms)[0]
ctype = room.check_connection((add_orig_point.x, add_orig_point.y), (add_dest_point.x, add_dest_point.y))
if ctype is not None:
from_point = GraphPoint(add_orig_point.x, add_orig_point.y, room)
to_point = GraphPoint(add_dest_point.x, add_dest_point.y, room)
return Route((GraphConnection(from_point, to_point, ctype=ctype), ))
if common_points:
# same location
if not add_orig_point and not add_dest_point:
raise AlreadyThere()
# points are connectable with only one via
best_point = None
best_distance = np.inf
for point in common_points:
distance = 0
if add_orig_point:
distance += abs(np.linalg.norm(add_orig_point.xy - point.xy))
if add_dest_point:
distance += abs(np.linalg.norm(add_dest_point.xy - point.xy))
if distance < best_distance:
best_distance = distance
best_point = point
connections = []
if add_orig_point:
from_point = GraphPoint(add_orig_point.x, add_orig_point.y, best_point.room)
ctype = orig_ctypes[tuple(orig_points_i).index(best_point.i)]
connections.append(GraphConnection(from_point, best_point, ctype=ctype))
if add_dest_point:
to_point = GraphPoint(add_dest_point.x, add_dest_point.y, best_point.room)
ctype = dest_ctypes[tuple(dest_points_i).index(best_point.i)]
connections.append(GraphConnection(best_point, to_point, ctype=ctype))
return Route(connections)
# get origin points for each room (points as point index within room)
orig_room_points = {room: self._allowed_points_index(room.points, orig_points_i) for room in orig_rooms}
dest_room_points = {room: self._allowed_points_index(room.points, dest_points_i) for room in dest_rooms}
# add distances to room routers
if orig_distances is not None:
for room in orig_rooms:
distances = np.array(tuple(orig_distances[room.points[i]] for i in orig_room_points[room]))
routers[room].shortest_paths[orig_room_points[room], :] += distances[:, None]
if dest_distances is not None:
for room in dest_rooms:
distances = np.array(tuple(dest_distances[room.points[i]] for i in dest_room_points[room]))
routers[room].shortest_paths[:, dest_room_points[room]] += distances
# if the points have common rooms, search for routes within those rooms
if common_rooms:
for room in common_rooms:
shortest_paths = routers[room].shortest_paths[orig_room_points[room][:, None],
dest_room_points[room]]
distance = shortest_paths.min()
# Is this route better than the previous ones?
if distance >= best_route.distance:
continue
# noinspection PyTypeChecker
from_point, to_point = np.argwhere(shortest_paths == distance)[0]
from_point = orig_room_points[room][from_point]
to_point = dest_room_points[room][to_point]
best_route = RoomRouteSegment(room, routers, from_point, to_point).as_route()
# get reachable room transfer points and their distance
# as a dictionary: global transfer point index => RoomRouteSegment
orig_room_transfers = self._room_transfers(orig_rooms, orig_room_points, routers, mode='orig')
dest_room_transfers = self._room_transfers(dest_rooms, dest_room_points, routers, mode='dest')
# route within level
orig_levels = set(room.level for room in orig_rooms)
dest_levels = set(room.level for room in dest_rooms)
common_levels = orig_levels & dest_levels
# get reachable roomtransfer points for each level (points as room transfer point index within level)
orig_room_transfer_points = {level: self._allowed_points_index(level.room_transfer_points, orig_room_transfers)
for level in orig_levels}
dest_room_transfer_points = {level: self._allowed_points_index(level.room_transfer_points, dest_room_transfers)
for level in dest_levels}
# if the points have common levels, search for routes within those levels
if common_levels:
for level in common_levels:
o_points = orig_room_transfer_points[level]
d_points = dest_room_transfer_points[level]
if not len(o_points) or not len(d_points):
continue
shortest_paths = routers[level].shortest_paths[o_points[:, None], d_points]
# add distances to the the room transfer points to the rows and columns
shortest_paths += np.array(tuple(orig_room_transfers[level.room_transfer_points[in_level_i]].distance
for in_level_i in o_points))[:, None]
shortest_paths += np.array(tuple(dest_room_transfers[level.room_transfer_points[in_level_i]].distance
for in_level_i in d_points))
distance = shortest_paths.min()
# Is this route better than the previous ones?
if distance >= best_route.distance:
continue
# noinspection PyTypeChecker
from_point, to_point = np.argwhere(shortest_paths == distance)[0]
from_point = o_points[from_point]
to_point = d_points[to_point]
best_route = SegmentRoute((orig_room_transfers[level.room_transfer_points[from_point]],
LevelRouteSegment(level, routers, from_point, to_point),
dest_room_transfers[level.room_transfer_points[to_point]]),
distance=distance)
# get reachable level transfer points and their distance
# as a dictionary: global transfer point index => Route
orig_level_transfers = self._level_transfers(orig_levels, orig_room_transfers, routers, mode='orig')
dest_level_transfers = self._level_transfers(dest_levels, dest_room_transfers, routers, mode='dest')
# get reachable leveltransfer points (points as level transfer point index within graph)
orig_level_transfer_points = self._allowed_points_index(self.level_transfer_points, orig_level_transfers)
dest_level_transfer_points = self._allowed_points_index(self.level_transfer_points, dest_level_transfers)
# search a route within the whole graph
o_points = orig_level_transfer_points
d_points = dest_level_transfer_points
if len(o_points) and len(d_points):
shortest_paths = routers[self].shortest_paths[o_points[:, None], d_points]
# add distances to the the room transfer points to the rows and columns
shortest_paths += np.array(tuple(orig_level_transfers[self.level_transfer_points[in_graph_i]].distance
for in_graph_i in o_points))[:, None]
shortest_paths += np.array(tuple(dest_level_transfers[self.level_transfer_points[in_graph_i]].distance
for in_graph_i in d_points))
distance = shortest_paths.min()
# Is this route better than the previous ones?
if distance < best_route.distance:
# noinspection PyTypeChecker
from_point, to_point = np.argwhere(shortest_paths == distance)[0]
from_point = o_points[from_point]
to_point = d_points[to_point]
best_route = SegmentRoute((orig_level_transfers[self.level_transfer_points[from_point]],
GraphRouteSegment(self, routers, from_point, to_point),
dest_level_transfers[self.level_transfer_points[to_point]]),
distance=distance)
if best_route is NoRoute:
raise NoRouteFound()
orig_ctype = orig_ctypes[tuple(orig_points_i).index(best_route.from_point)] if add_orig_point else None
dest_ctype = dest_ctypes[tuple(dest_points_i).index(best_route.to_point)] if add_dest_point else None
best_route = SegmentRouteWrapper(best_route, orig_point=add_orig_point, dest_point=add_dest_point,
orig_ctype=orig_ctype, dest_ctype=dest_ctype)
best_route = best_route.split()
return best_route
def _room_transfers(self, rooms, room_points, routers, mode):
if mode not in ('orig', 'dest'):
raise ValueError
room_transfers = {}
for room in rooms:
room_transfer_points = np.array(tuple(room.points.index(point)
for point in room.room_transfer_points), dtype=int)
points = room_points[room]
if mode == 'orig':
shortest_paths = routers[room].shortest_paths[points[:, None], room_transfer_points]
else:
shortest_paths = routers[room].shortest_paths[room_transfer_points[:, None], points]
# noinspection PyTypeChecker
for from_i, to_i in np.argwhere(shortest_paths != np.inf):
distance = shortest_paths[from_i, to_i]
from_i = points[from_i] if mode == 'orig' else room_transfer_points[from_i]
to_i = room_transfer_points[to_i] if mode == 'orig' else points[to_i]
transfer_i = room.points[to_i if mode == 'orig' else from_i]
if transfer_i not in room_transfers or room_transfers[transfer_i].distance > distance:
room_transfers[transfer_i] = RoomRouteSegment(room, routers, from_i, to_i)
return room_transfers
def _level_transfers(self, levels, all_room_transfers, routers, mode):
if mode not in ('orig', 'dest'):
raise ValueError
level_transfers = {}
for level in levels:
level_transfer_points = np.array(tuple(level.room_transfer_points.index(point)
for point in level.level_transfer_points), dtype=int)
points, distances = zip(*(tuple((level.room_transfer_points.index(point), segment.distance)
for point, segment in all_room_transfers.items()
if point in level.room_transfer_points)))
points = np.array(points)
distances = np.array(distances)
if mode == 'orig':
shortest_paths = routers[level].shortest_paths[points[:, None], level_transfer_points]
shortest_paths += distances[:, None]
else:
shortest_paths = routers[level].shortest_paths[level_transfer_points[:, None], points]
shortest_paths += distances
# noinspection PyTypeChecker
for from_i, to_i in np.argwhere(shortest_paths != np.inf):
distance = shortest_paths[from_i, to_i]
from_i = points[from_i] if mode == 'orig' else level_transfer_points[from_i]
to_i = level_transfer_points[to_i] if mode == 'orig' else points[to_i]
transfer_i = level.room_transfer_points[to_i if mode == 'orig' else from_i]
room_transfer_i = level.room_transfer_points[from_i if mode == 'orig' else to_i]
if transfer_i not in level_transfers or level_transfers[transfer_i].distance > distance:
segments = [LevelRouteSegment(level, routers, from_i, to_i)]
if mode == 'orig':
segments.insert(0, all_room_transfers[room_transfer_i])
else:
segments.append(all_room_transfers[room_transfer_i])
level_transfers[transfer_i] = SegmentRoute(segments, distance)
return level_transfers
def get_nearest_point(self, level, x, y):
return self.levels[level.name].nearest_point(np.array((x, y)), 'orig')
GraphRouter = namedtuple('GraphRouter', ('shortest_paths', 'predecessors', 'level_transfers', ))

430
src/c3nav/routing/level.py
View file

@ -1,430 +0,0 @@
import os
from collections import namedtuple
import numpy as np
from django.conf import settings
from django.core.cache import cache
from matplotlib.path import Path
from PIL import Image, ImageDraw
from scipy.sparse.csgraph._shortest_path import shortest_path
from scipy.sparse.csgraph._tools import csgraph_from_dense
from shapely.geometry import CAP_STYLE, JOIN_STYLE, LineString
from c3nav.mapdata.utils.geometry import assert_multilinestring, assert_multipolygon
from c3nav.mapdata.utils.misc import get_public_private_area
from c3nav.mapdata.utils.mpl import shapely_to_mpl
from c3nav.routing.point import GraphPoint
from c3nav.routing.room import GraphRoom
from c3nav.routing.utils.base import get_nearest_point
from c3nav.routing.utils.coords import coord_angle
from c3nav.routing.utils.draw import _ellipse_bbox, _line_coords
class GraphLevel():
def __init__(self, graph, level):
self.graph = graph
self.level = level
self.rooms = []
self.points = []
self.room_transfer_points = None
self.level_transfer_points = None
self.arealocation_points = None
def serialize(self):
return (
[room.serialize() for room in self.rooms],
self.points,
self.room_transfer_points,
self.level_transfer_points,
self.arealocation_points,
)
def unserialize(self, data):
rooms, self.points, self.room_transfer_points, self.level_transfer_points, self.arealocation_points = data
self.rooms = tuple(GraphRoom.unserialize(self, room) for room in rooms)
# Building the Graph
def build(self):
print()
print('Level %s:' % self.level.name)
self._built_points = []
self._built_room_transfer_points = []
self.collect_stairs()
self.collect_escalators()
self.collect_rooms()
print('%d rooms' % len(self.rooms))
for room in self.rooms:
room.build_areas()
room.build_points()
self.create_doors()
self.create_oneways()
self.create_levelconnectors()
self.create_elevatorlevels()
self.collect_arealocations()
self.collect_stuffedareas()
self._built_points = sum((room._built_points for room in self.rooms), [])
self._built_points.extend(self._built_room_transfer_points)
for room in self.rooms:
room.build_connections()
print('%d excludables' % len(self._built_excludables))
print('%d points' % len(self._built_points))
print('%d room transfer points' % len(self._built_room_transfer_points))
print('%d area locations' % len(self._built_arealocations))
def connection_count(self):
return sum(room.connection_count() for room in self.rooms)
def collect_stairs(self):
self.mpl_stairs = ()
for stair_line in assert_multilinestring(self.level.geometries.stairs):
coords = tuple(stair_line.coords)
self.mpl_stairs += tuple((Path(part), coord_angle(*part)) for part in zip(coords[:-1], coords[1:]))
def collect_escalators(self):
self.mpl_escalatorslopes = ()
for escalatorslope_line in assert_multilinestring(self.level.geometries.escalatorslopes):
coords = tuple(escalatorslope_line.coords)
self.mpl_escalatorslopes += tuple((Path(part), coord_angle(*part))
for part in zip(coords[:-1], coords[1:]))
self._built_escalators = []
for escalator in self.level.escalators.all():
mpl_escalator = shapely_to_mpl(escalator.geometry)
for slope_line, angle in self.mpl_escalatorslopes:
if mpl_escalator.intersects_path(slope_line, filled=True):
self._built_escalators.append(EscalatorData(mpl_escalator, escalator.direction, slope_line, angle))
break
else:
print('Escalator %s has no slope line!' % escalator.name)
continue
def collect_oneways(self):
self._built_oneways = ()
for oneway_line in assert_multilinestring(self.level.geometries.oneways):
coords = tuple(oneway_line.coords)
self._built_oneways += tuple((Path(part), coord_angle(*part))
for part in zip(coords[:-1], coords[1:]))
def collect_rooms(self):
accessibles = self.level.geometries.accessible_without_oneways
accessibles = assert_multipolygon(accessibles)
for geometry in accessibles:
room = GraphRoom(self)
if room.prepare_build(geometry):
self.rooms.append(room)
def collect_arealocations(self):
self._built_arealocations = {}
self._built_excludables = {}
for excludable in self.level.arealocations.all():
self._built_arealocations[excludable.name] = excludable.geometry
if excludable.routing_inclusion != 'default' or not excludable.public:
self._built_excludables[excludable.name] = excludable.geometry
public_area, private_area = get_public_private_area(self.level)
self._built_arealocations[':public'] = public_area
self._built_excludables[':public'] = public_area
self._built_arealocations[':nonpublic'] = private_area
self._built_excludables[':nonpublic'] = private_area
# add points inside arealocations to be able to route to its borders
for excludable in self._built_arealocations.values():
smaller = excludable.buffer(-0.05, join_style=JOIN_STYLE.mitre)
for room in self.rooms:
room.add_points_on_rings(assert_multipolygon(smaller))
# add points outside excludables so if excluded you can walk around them
for excludable in self._built_excludables.values():
for polygon in assert_multipolygon(excludable.buffer(0.28, join_style=JOIN_STYLE.mitre)):
for room in self.rooms:
room._add_ring(polygon.exterior, want_left=True)
for interior in polygon.interiors:
room._add_ring(interior, want_left=False)
def collect_stuffedareas(self):
self._built_stuffedareas = self.level.geometries.stuffedareas
for polygon in assert_multipolygon(self._built_stuffedareas.buffer(0.05, join_style=JOIN_STYLE.mitre)):
for room in self.rooms:
room._add_ring(polygon.exterior, want_left=True)
for interior in polygon.interiors:
room._add_ring(interior, want_left=False)
def create_doors(self):
doors = self.level.geometries.doors
doors = assert_multipolygon(doors)
for door in doors:
polygon = door.buffer(0.01, join_style=JOIN_STYLE.mitre)
center = door.centroid
num_points = 0
connected_rooms = set()
points = []
for room in self.rooms:
if not polygon.intersects(room._built_geometry):
continue
for subpolygon in assert_multipolygon(polygon.intersection(room._built_geometry)):
connected_rooms.add(room)
nearest_point = get_nearest_point(room.clear_geometry, subpolygon.centroid)
point, = room.add_point(nearest_point.coords[0])
points.append(point)
if len(points) < 2:
print('door with <2 points (%d) detected at (%.2f, %.2f)' % (num_points, center.x, center.y))
continue
center_point = GraphPoint(center.x, center.y, None)
self._built_room_transfer_points.append(center_point)
for room in connected_rooms:
room._built_points.append(center_point)
for point in points:
center_point.connect_to(point)
point.connect_to(center_point)
def create_oneways(self):
oneways = self.level.geometries.oneways
oneways = assert_multilinestring(oneways)
segments = ()
for oneway in oneways:
coords = tuple(oneway.coords)
segments += tuple((Path(part), coord_angle(*part))
for part in zip(coords[:-1], coords[1:]))
for oneway, oneway_angle in segments:
line_string = LineString(tuple(oneway.vertices))
polygon = line_string.buffer(0.10, join_style=JOIN_STYLE.mitre, cap_style=CAP_STYLE.flat)
center = polygon.centroid
num_points = 0
connected_rooms = set()
points = []
for room in self.rooms:
if not polygon.intersects(room._built_geometry):
continue
for subpolygon in assert_multipolygon(polygon.intersection(room._built_geometry)):
connected_rooms.add(room)
nearest_point = get_nearest_point(room.clear_geometry, subpolygon.centroid)
point, = room.add_point(nearest_point.coords[0])
points.append(point)
if len(points) < 2:
print('oneway with <2 points (%d) detected at (%.2f, %.2f)' % (num_points, center.x, center.y))
continue
center_point = GraphPoint(center.x, center.y, None)
self._built_room_transfer_points.append(center_point)
for room in connected_rooms:
room._built_points.append(center_point)
for point in points:
angle = coord_angle(point.xy, center_point.xy)
angle_diff = ((oneway_angle - angle + 180) % 360) - 180
direction_up = (angle_diff > 0)
if direction_up:
point.connect_to(center_point)
else:
center_point.connect_to(point)
def create_levelconnectors(self):
for levelconnector in self.level.levelconnectors.all():
polygon = levelconnector.geometry
for room in self.rooms:
if not polygon.intersects(room._built_geometry):
continue
for subpolygon in assert_multipolygon(polygon.intersection(room._built_geometry)):
point = subpolygon.centroid
if not point.within(room.clear_geometry):
point = get_nearest_point(room.clear_geometry, point)
point, = room.add_point(point.coords[0])
self.graph.add_levelconnector_point(levelconnector, point)
def create_elevatorlevels(self):
for elevatorlevel in self.level.elevatorlevels.all():
center = elevatorlevel.geometry.centroid
mpl_elevatorlevel = shapely_to_mpl(elevatorlevel.geometry)
for room in self.rooms:
if not room.mpl_clear.contains_point(center.coords[0]):
continue
room._built_is_elevatorlevel = True
points = [point for point in room._built_points if mpl_elevatorlevel.contains_point(point.xy)]
if not points:
print('elevatorlevel %s has 0 points!' % (elevatorlevel.name))
break
elif len(points) > 1:
print('elevatorlevel %s has > 2 points!' % (elevatorlevel.name))
break
point = points[0]
self.graph.add_elevatorlevel_point(elevatorlevel, point)
break
def finish_build(self):
self.rooms = tuple(self.rooms)
self.points = tuple(point.i for point in self._built_points)
self.room_transfer_points = tuple(point.i for point in self._built_room_transfer_points)
self.level_transfer_points = tuple(i for i in self.points if i in self.graph.level_transfer_points)
self.collect_arealocation_points()
for room in self.rooms:
room.finish_build()
def collect_arealocation_points(self):
self.arealocation_points = {}
for room in self.rooms:
room.excludables = []
for name, arealocation in self._built_arealocations.items():
mpl_area = shapely_to_mpl(arealocation)
rooms = [room for room in self.rooms
if any(room.mpl_clear.intersects_path(exterior, filled=True) for exterior in mpl_area.exteriors)]
possible_points = tuple(point for point in sum((room._built_points for room in rooms), []) if point.room)
points = tuple(point for point in possible_points if mpl_area.contains_point(point.xy))
self.arealocation_points[name] = tuple(point.i for point in points)
if name in self._built_excludables:
for room in set(point.room for point in points):
room.excludables.append(name)
# Drawing
ctype_colors = {
'': (50, 200, 0),
'stairs_up': (255, 50, 50),
'stairs_down': (255, 50, 50),
'escalator_up': (255, 150, 0),
'escalator_down': (200, 100, 0),
'elevator_up': (200, 0, 200),
'elevator_down': (200, 0, 200),
}
def draw_png(self, points=True, lines=True):
filename = os.path.join(settings.RENDER_ROOT, 'base-level-%s.png' % self.level.name)
graph_filename = os.path.join(settings.RENDER_ROOT, 'graph-level-%s.png' % self.level.name)
im = Image.open(filename)
height = im.size[1]
draw = ImageDraw.Draw(im)
if lines:
for room in self.rooms:
# noinspection PyTypeChecker
for ctype, from_i, to_i in np.argwhere(room.distances != np.inf):
draw.line(_line_coords(self.graph.points[room.points[from_i]],
self.graph.points[room.points[to_i]], height),
fill=self.ctype_colors[room.ctypes[ctype]])
if points:
for point_i in self.points:
point = self.graph.points[point_i]
draw.ellipse(_ellipse_bbox(point.x, point.y, height), (200, 0, 0))
for point_i in self.room_transfer_points:
point = self.graph.points[point_i]
draw.ellipse(_ellipse_bbox(point.x, point.y, height), (0, 0, 255))
for point_i in self.level_transfer_points:
point = self.graph.points[point_i]
draw.ellipse(_ellipse_bbox(point.x, point.y, height), (0, 180, 0))
if lines:
for room in self.rooms:
# noinspection PyTypeChecker
for ctype, from_i, to_i in np.argwhere(room.distances != np.inf):
if room.points[from_i] in room.room_transfer_points:
draw.line(_line_coords(self.graph.points[room.points[from_i]],
self.graph.points[room.points[to_i]], height), fill=(0, 255, 255))
im.save(graph_filename)
# Routing
def build_routers(self, allowed_ctypes, allow_nonpublic, avoid, include):
routers = {}
empty_distances = np.empty(shape=(len(self.room_transfer_points),) * 2, dtype=np.float16)
empty_distances[:] = np.inf
sparse_distances = empty_distances.copy()
room_transfers = np.zeros(shape=(len(self.room_transfer_points),) * 2, dtype=np.int16)
room_transfers[:] = -1
for i, room in enumerate(self.rooms):
router = room.build_router(allowed_ctypes, allow_nonpublic, avoid, include)
routers[room] = router
room_distances = empty_distances.copy()
in_room_i = np.array(tuple(room.points.index(point) for point in room.room_transfer_points), dtype=int)
in_level_i = np.array(tuple(self.room_transfer_points.index(point)
for point in room.room_transfer_points), dtype=int)
room_distances[in_level_i[:, None], in_level_i] = router.shortest_paths[in_room_i[:, None], in_room_i]
better = room_distances < sparse_distances
sparse_distances[better] = room_distances[better]
room_transfers[better] = i
g_sparse = csgraph_from_dense(sparse_distances, null_value=np.inf)
shortest_paths, predecessors = shortest_path(g_sparse, return_predecessors=True)
routers[self] = LevelRouter(shortest_paths, predecessors, room_transfers)
return routers
def nearest_point(self, point, mode):
cache_key = ('c3nav__routing__nearest_point__%s__%s__%.2f_%.2f__%s' %
(self.graph.mtime, self.level.name, point[0], point[1], mode))
nearest_point = cache.get(cache_key, None)
if nearest_point is None:
nearest_point = self._nearest_point(point, mode)
cache.set(cache_key, nearest_point, 60)
if nearest_point is None:
return None
return self.graph.points[nearest_point]
def _nearest_point(self, point, mode):
points = self.connected_points(point, mode)
if not points:
return None
nearest_point = min(points.items(), key=lambda x: x[1][0])
return nearest_point[0]
def connected_points(self, point, mode):
cache_key = ('c3nav__routing__connected_points__%s__%s__%.2f_%.2f__%s' %
(self.graph.mtime, self.level.name, point[0], point[1], mode))
points = cache.get(cache_key, None)
if points is None or True:
points = self._connected_points(point, mode)
cache.set(cache_key, points, 60)
return points
def _connected_points(self, point, mode):
for room in self.rooms:
if room.contains_point(point):
return room.connected_points(point, mode)
return {}
LevelRouter = namedtuple('LevelRouter', ('shortest_paths', 'predecessors', 'room_transfers', ))
EscalatorData = namedtuple('EscalatorData', ('mpl_geom', 'direction_up', 'slope', 'angle'))

0
src/c3nav/routing/management/__init__.py
View file

0
src/c3nav/routing/management/commands/__init__.py
View file

24
src/c3nav/routing/management/commands/buildgraph.py
View file

@ -1,24 +0,0 @@
import time
from django.core.management.base import BaseCommand
from c3nav.routing.graph import Graph
class Command(BaseCommand):
help = 'build the routing graph'
def handle(self, *args, **options):
start = time.time()
graph = Graph()
graph.build()
print()
print('Built in %.4fs' % (time.time() - start))
start = time.time()
graph.save()
print('Saved in %.4fs' % (time.time()-start))
start = time.time()
Graph.load()
print('Loaded in %.4fs' % (time.time() - start))

18
src/c3nav/routing/management/commands/drawgraph.py
View file

@ -1,18 +0,0 @@
from django.core.management.base import BaseCommand
from c3nav.routing.graph import Graph
class Command(BaseCommand):
help = 'draw the routing graph'
def add_arguments(self, parser):
parser.add_argument('--no-points', action='store_const', dest='points', const=False, default=True,
help='dont draw points on the graph image')
parser.add_argument('--no-lines', action='store_const', dest='lines', const=False, default=True,
help='dont draw lines on the graph image')
def handle(self, *args, **options):
graph = Graph.load()
graph.draw_pngs(points=options['points'], lines=options['lines'])

22
src/c3nav/routing/management/commands/testgraph.py
View file

@ -1,22 +0,0 @@
import time
from django.core.management.base import BaseCommand
from c3nav.routing.graph import Graph
class Command(BaseCommand):
help = 'check how long it takes to build the routers for the routing graph'
def handle(self, *args, **options):
start = time.time()
graph = Graph.load()
print('Graph loaded in %.4fs' % (time.time() - start))
start = time.time()
graph.build_routers()
print('Routers built in %.4fs' % (time.time() - start))
start = time.time()
graph.build_routers()
print('Routers built (2nd time, cached) in %.4fs' % (time.time() - start))

46
src/c3nav/routing/point.py
View file

@ -1,46 +0,0 @@
import numpy as np
from django.conf import settings
from django.utils.functional import cached_property
from c3nav.routing.connection import GraphConnection
class GraphPoint():
def __init__(self, x, y, room):
self.x = x
self.y = y
self.room = room
self.xy = np.array((x, y))
self.i = None
self.connections = {}
self.connections_in = {}
@cached_property
def level(self):
return self.room and self.room.level
def serialize(self):
return (
self.x,
self.y,
None if self.room is None else self.room.i,
)
@cached_property
def ellipse_bbox(self):
x = self.x * settings.RENDER_SCALE
y = self.y * settings.RENDER_SCALE
return ((x-5, y-5), (x+5, y+5))
def connect_to(self, other_point, ctype='', distance=None):
connection = GraphConnection(self, other_point, ctype=ctype, distance=distance)
self.connections[other_point] = connection
other_point.connections_in[self] = connection
@cached_property
def arealocations(self):
return tuple(name for name, points_i in self.level.arealocation_points.items() if self.i in points_i)
def __repr__(self):
return '<GraphPoint x=%f y=%f room=%s>' % (self.x, self.y, (id(self.room) if self.room else None))

332
src/c3nav/routing/room.py
View file

@ -1,332 +0,0 @@
from collections import namedtuple
import numpy as np
from matplotlib.path import Path
from scipy.sparse.csgraph._shortest_path import shortest_path
from scipy.sparse.csgraph._tools import csgraph_from_dense
from shapely.geometry import CAP_STYLE, JOIN_STYLE, LineString
from shapely.ops import cascaded_union
from c3nav.mapdata.utils.geometry import assert_multilinestring, assert_multipolygon
from c3nav.mapdata.utils.mpl import shapely_to_mpl
from c3nav.routing.area import GraphArea
from c3nav.routing.connection import GraphConnection
from c3nav.routing.point import GraphPoint
from c3nav.routing.utils.coords import get_coords_angles
class GraphRoom():
def __init__(self, level):
self.level = level
self.graph = level.graph
self.mpl_clear = None
self.i = None
self.areas = []
self.points = None
self.room_transfer_points = None
self.distances = np.zeros((1, ))
self.ctypes = None
self.excludables = None
self.stuffedareas = None
def serialize(self):
return (
self.mpl_clear,
[area.serialize() for area in self.areas],
self.points,
self.room_transfer_points,
self.distances,
self.ctypes,
self.excludables,
self.excludable_points,
self.stuffedareas,
)
@classmethod
def unserialize(cls, level, data):
room = cls(level)
(room.mpl_clear, areas, room.points, room.room_transfer_points,
room.distances, room.ctypes, room.excludables, room.excludable_points, room.stuffedareas) = data
room.areas = tuple(GraphArea(room, *area) for area in areas)
return room
# Building the Graph
def prepare_build(self, geometry):
self._built_geometry = geometry
self.clear_geometry = self._built_geometry.buffer(-0.3, join_style=JOIN_STYLE.mitre)
if self.clear_geometry.is_empty:
return False
self._built_points = []
self._built_is_elevatorlevel = False
self.mpl_clear = shapely_to_mpl(self.clear_geometry.buffer(0.01, join_style=JOIN_STYLE.mitre))
self.mpl_stairs = tuple((stair, angle) for stair, angle in self.level.mpl_stairs
if self.mpl_clear.intersects_path(stair, filled=True))
self._built_escalators = tuple(escalator for escalator in self.level._built_escalators
if self.mpl_clear.intersects_path(escalator.mpl_geom.exterior, filled=True))
self.isolated_areas = []
return True
def build_areas(self):
stairs_areas = self.level.level.geometries.stairs
stairs_areas = stairs_areas.buffer(0.3, join_style=JOIN_STYLE.mitre, cap_style=CAP_STYLE.flat)
stairs_areas = stairs_areas.intersection(self._built_geometry)
self._built_isolated_areas = tuple(assert_multipolygon(stairs_areas))
escalators_areas = self.level.level.geometries.escalators
escalators_areas = escalators_areas.intersection(self._built_geometry)
self._built_isolated_areas += tuple(assert_multipolygon(escalators_areas))
escalators_and_stairs = cascaded_union((stairs_areas, escalators_areas))
isolated_areas = tuple(assert_multipolygon(stairs_areas.intersection(self.clear_geometry)))
isolated_areas += tuple(assert_multipolygon(escalators_areas.intersection(self.clear_geometry)))
isolated_areas += tuple(assert_multipolygon(self.clear_geometry.difference(escalators_and_stairs)))
for isolated_area in isolated_areas:
mpl_clear = shapely_to_mpl(isolated_area.buffer(0.01, join_style=JOIN_STYLE.mitre))
mpl_stairs = tuple((stair, angle) for stair, angle in self.mpl_stairs
if mpl_clear.intersects_path(stair, filled=True))
escalators = tuple(escalator for escalator in self._built_escalators
if escalator.mpl_geom.intersects_path(mpl_clear.exterior, filled=True))
area = GraphArea(self, mpl_clear, mpl_stairs, escalators)
area.prepare_build()
self.areas.append(area)
def build_points(self):
narrowed_geometry = self._built_geometry.buffer(-0.6, join_style=JOIN_STYLE.mitre)
geometry = narrowed_geometry.buffer(0.31, join_style=JOIN_STYLE.mitre).intersection(self.clear_geometry)
if geometry.is_empty:
return
# points with 60cm distance to borders
polygons = assert_multipolygon(geometry)
for polygon in polygons:
self._add_ring(polygon.exterior, want_left=False)
for interior in polygon.interiors:
self._add_ring(interior, want_left=True)
# now fill in missing doorways or similar
accessible_clear_geometry = geometry.buffer(0.31, join_style=JOIN_STYLE.mitre)
missing_geometry = self.clear_geometry.difference(accessible_clear_geometry)
polygons = assert_multipolygon(missing_geometry)
for polygon in polygons:
overlaps = polygon.buffer(0.02).intersection(accessible_clear_geometry)
if overlaps.is_empty:
continue
points = []
# overlaps to non-missing areas
overlaps = assert_multipolygon(overlaps)
for overlap in overlaps:
points += self.add_point(overlap.centroid.coords[0])
points += self._add_ring(polygon.exterior, want_left=False)
for interior in polygon.interiors:
points += self._add_ring(interior, want_left=True)
# points around steps
self.add_points_on_rings(self._built_isolated_areas)
def _add_ring(self, geom, want_left):
"""
add the points of a ring, but only those that have a specific direction change.
additionally removes unneeded points if the neighbors can be connected in self.clear_geometry
:param geom: LinearRing
:param want_left: True if the direction has to be left, False if it has to be right
"""
coords = []
skipped = False
can_delete_last = False
for coord, is_left in get_coords_angles(geom):
if is_left != want_left:
skipped = True
continue
if not skipped and can_delete_last and len(coords) >= 2:
if LineString((coords[-2], coord)).within(self.clear_geometry):
coords[-1] = coord
continue
coords.append(coord)
can_delete_last = not skipped
skipped = False
if not skipped and can_delete_last and len(coords) >= 3:
if LineString((coords[-2], coords[0])).within(self.clear_geometry):
coords.pop()
points = []
for coord in coords:
points += self.add_point(coord)
return points
def add_points_on_rings(self, areas):
for polygon in areas:
for ring in (polygon.exterior,) + tuple(polygon.interiors):
for linestring in assert_multilinestring(ring.intersection(self.clear_geometry)):
coords = tuple(linestring.coords)
if len(coords) == 2:
path = Path(coords)
length = abs(np.linalg.norm(path.vertices[0] - path.vertices[1]))
for coord in tuple(path.interpolated(int(length / 1.0 + 1)).vertices):
self.add_point(coord)
continue
start = 0
for segment in zip(coords[:-1], coords[1:]):
path = Path(segment)
length = abs(np.linalg.norm(path.vertices[0] - path.vertices[1]))
if length < 1.0:
coords = (path.vertices[1 if start == 0 else 0],)
else:
coords = tuple(path.interpolated(int(length / 1.0 + 0.5)).vertices)[start:]
for coord in coords:
self.add_point(coord)
start = 1
def add_point(self, coord):
if not self.mpl_clear.contains_point(coord):
return []
point = GraphPoint(coord[0], coord[1], self)
self._built_points.append(point)
for area in self.areas:
area.add_point(point)
return [point]
def build_connections(self):
if self._built_is_elevatorlevel:
return
self.stuffedareas = shapely_to_mpl(self.level._built_stuffedareas.intersection(self._built_geometry))
for area in self.areas:
area.build_connections()
def connection_count(self):
return np.count_nonzero(self.distances >= 0)
def finish_build(self):
self.areas = tuple(self.areas)
self.points = tuple(point.i for point in self._built_points)
set_points = set(self.points)
if len(self.points) != len(set_points):
print('ERROR: POINTS DOUBLE-ADDED (ROOM)', len(self.points), len(set_points))
self.room_transfer_points = tuple(i for i in self.points if i in self.level.room_transfer_points)
self.excludables = tuple(self.excludables)
excludable_points = list()
for excludable in self.excludables:
points = self.level.arealocation_points[excludable]
excludable_points.append(np.array(tuple((i in points) for i in self.points)))
self.excludable_points = np.array(excludable_points)
mapping = {point.i: i for i, point in enumerate(self._built_points)}
empty = np.empty(shape=(len(self._built_points), len(self._built_points)), dtype=np.float16)
empty[:] = np.inf
ctypes = []
distances = {}
for from_point in self._built_points:
for to_point, connection in from_point.connections.items():
if to_point.i in mapping:
if connection.ctype not in distances:
ctypes.append(connection.ctype)
distances[connection.ctype] = empty.copy()
distances[connection.ctype][mapping[from_point.i], mapping[to_point.i]] = connection.distance
self.ctypes = tuple(ctypes)
self.distances = np.array(tuple(distances[ctype] for ctype in ctypes))
for area in self.areas:
area.finish_build()
# Routing
router_cache = {}
def build_router(self, allowed_ctypes, allow_nonpublic, avoid, include):
ctypes = tuple(i for i, ctype in enumerate(self.ctypes) if ctype in allowed_ctypes)
avoid = tuple(i for i, excludable in enumerate(self.excludables) if excludable in avoid)
include = tuple(i for i, excludable in enumerate(self.excludables) if excludable in include)
cache_key = ('c3nav__graph__roomrouter__%s__%s__%s__%d__%s__%s' %
(self.graph.mtime, self.i, ','.join(str(i) for i in ctypes),
allow_nonpublic, ','.join(str(i) for i in avoid), ','.join(str(i) for i in include)))
roomrouter = self.router_cache.get(cache_key)
if not roomrouter:
roomrouter = self._build_router(ctypes, allow_nonpublic, avoid, include)
self.router_cache[cache_key] = roomrouter
return roomrouter
def _build_router(self, ctypes, allow_nonpublic, avoid, include):
ctype_factors = np.ones((len(self.ctypes), 1, 1))*1000
ctype_factors[ctypes, :, :] = 1
if not self.distances.size:
return RoomRouter(np.ones((0, 0), dtype=int), np.ones((0, 0), dtype=int))
distances = np.amin(self.distances*ctype_factors, axis=0).astype(np.float32)
factors = np.ones_like(distances, dtype=np.float16)
if ':nonpublic' in self.excludables and ':nonpublic' not in include:
points, = self.excludable_points[self.excludables.index(':nonpublic')].nonzero()
factors[points[:, None], :] = 1000 if allow_nonpublic else np.inf
factors[:, points] = 1000 if allow_nonpublic else np.inf
if avoid:
points, = self.excludable_points[avoid, :].any(axis=0).nonzero()
factors[points[:, None], :] = np.maximum(factors[points[:, None], :], 1000)
factors[:, points] = np.maximum(factors[:, points], 1000)
if include:
points, = self.excludable_points[include, :].any(axis=0).nonzero()
factors[points[:, None], :] = 1
factors[:, points] = 1
g_sparse = csgraph_from_dense(distances*factors, null_value=np.inf)
shortest_paths, predecessors = shortest_path(g_sparse, return_predecessors=True)
return RoomRouter(shortest_paths, predecessors)
def get_connection(self, from_i, to_i):
stack = self.distances[:, from_i, to_i]
min_i = stack.argmin()
distance = stack[min_i]
ctype = self.ctypes[min_i]
return GraphConnection(self.graph.points[self.points[from_i]], self.graph.points[self.points[to_i]],
distance=distance, ctype=ctype)
def contains_point(self, point):
return self.mpl_clear.contains_point(point)
def connected_points(self, point, mode):
connections = {}
for area in self.areas:
if area.contains_point(point):
connections.update(area.connected_points(point, mode))
return connections
def check_connection(self, from_point, to_point):
from_point = np.array(from_point)
to_point = np.array(to_point)
for area in self.areas:
if area.contains_point(from_point) and area.contains_point(to_point):
there, back, distance = area.check_connection(from_point, to_point)
if there is not None:
return there
return None
RoomRouter = namedtuple('RoomRouter', ('shortest_paths', 'predecessors', ))

218
src/c3nav/routing/router.py Normal file
View file

@ -0,0 +1,218 @@
from collections import deque
import numpy as np
from django.utils.functional import cached_property
from shapely import prepared
from shapely.geometry import Point
from shapely.ops import unary_union
from c3nav.mapdata.models import AltitudeArea, GraphEdge, Level, WayType
class Router:
def __init__(self):
self.nodes = None
self.node_coords = None
self.node_lookup = None
@staticmethod
def get_altitude_in_areas(areas, point):
return max(area.get_altitudes(point)[0] for area in areas if area.geometry_prep.intersects(point))
@classmethod
def build(cls):
graph = cls()
levels_query = Level.objects.prefetch_related('buildings', 'spaces', 'altitudeareas',
'spaces__holes', 'spaces__columns',
'spaces__obstacles', 'spaces__lineobstacles',
'spaces__areas', 'spaces__graphnodes')
levels = {}
spaces = {}
areas = {}
nodes = deque()
for level in levels_query:
buildings_geom = unary_union(tuple(building.geometry for building in level.buildings.all()))
nodes_before_count = len(nodes)
for space in level.spaces.all():
# create space geometries
accessible_geom = space.geometry.difference(unary_union(
tuple(column.geometry for column in space.columns.all()) +
tuple(hole.geometry for hole in space.holes.all()) +
((buildings_geom, ) if space.outside else ())
))
obstacles_geom = unary_union( # noqa
tuple(obstacle.geometry for obstacle in space.obstacles.all()) +
tuple(lineobstacle.buffered_geometry for lineobstacle in space.lineobstacles.all())
)
# todo: do something with this, then remove #noqa
space_nodes = tuple(RouterNode.from_graph_node(node) for node in space.graphnodes.all())
for i, node in enumerate(space_nodes, start=len(nodes)):
node.i = i
nodes.extend(space_nodes)
for area in space.areas.all():
area = RouterArea(area)
area_nodes = tuple(node for node in space_nodes if area.geometry_prep.intersects(node.point))
area.nodes = set(node.i for node in area_nodes)
for node in area_nodes:
node.areas.add(area.pk)
areas[area.pk] = area
space._prefetched_objects_cache = {}
space = RouterSpace(space)
space.nodes = set(node.i for node in space_nodes)
for area in level.altitudeareas.all():
if not space.geometry_prep.intersects(area.geometry):
continue
area = RouterAltitudeArea(accessible_geom.intersection(area.geometry),
area.altitude, area.altitude2, area.point1, area.point2)
area_nodes = tuple(node for node in space_nodes if area.geometry_prep.intersects(node.point))
area.nodes = set(node.i for node in area_nodes)
for node in area_nodes:
altitude = area.get_altitude(node)
if node.altitude is None or node.altitude < altitude:
node.altitude = altitude
space.altitudeareas.append(area)
spaces[space.pk] = space
level_spaces = set(space.pk for space in level.spaces.all())
level._prefetched_objects_cache = {}
level = RouterLevel(level, spaces=level_spaces)
level.nodes = set(range(nodes_before_count, len(nodes)))
levels[level.pk] = level
# waytypes
waytypes = deque([RouterWayType(None)])
waytypes_lookup = {None: 0}
for i, waytype in enumerate(WayType.objects.all(), start=1):
waytypes.append(RouterWayType(waytype))
waytypes_lookup[waytype.pk] = i
waytypes = tuple(waytypes)
# collect nodes
nodes = tuple(nodes)
nodes_lookup = {node.pk: node.i for node in nodes}
nodes_coords = np.array(tuple((node.x*100, node.y*100) for node in nodes), dtype=np.uint32) # noqa
# todo: remove #noqa when we're ready
# collect edges
edges = tuple(RouterEdge(from_node=nodes[nodes_lookup[edge.from_node_id]],
to_node=nodes[nodes_lookup[edge.to_node_id]],
waytype=waytypes_lookup[edge.waytype_id]) for edge in GraphEdge.objects.all())
edges_lookup = {(edge.from_node.i, edge.to_node.i): edge for edge in edges} # noqa
# todo: remove #noqa when we're ready
# build graph matrix
graph = np.full(shape=(len(nodes), len(nodes)), fill_value=np.inf, dtype=np.float32)
for edge in edges:
index = (edge.from_node.i, edge.to_node.i)
graph[index] = edge.distance
waytype = waytypes[edge.waytype]
(waytype.upwards_indices if edge.rise > 0 else waytype.nonupwards_indices).append(index)
# finalize waytype matrixes
for waytype in waytypes:
waytype.upwards_indices = np.array(waytype.upwards_indices, dtype=np.uint32).reshape((-1, 2))
waytype.nonupwards_indices = np.array(waytype.nonupwards_indices, dtype=np.uint32).reshape((-1, 2))
class BaseRouterProxy:
def __init__(self, src):
self.src = src
self.nodes = set()
@cached_property
def geometry_prep(self):
return prepared.prep(self.src.geometry)
def __getstate__(self):
result = self.__dict__.copy()
result.pop('geometry_prep', None)
return result
def __getattr__(self, name):
return getattr(self.src, name)
class RouterLevel(BaseRouterProxy):
def __init__(self, level, spaces=None):
super().__init__(level)
self.spaces = spaces if spaces else set()
class RouterSpace(BaseRouterProxy):
def __init__(self, space, altitudeareas=None):
super().__init__(space)
self.altitudeareas = altitudeareas if altitudeareas else []
class RouterArea(BaseRouterProxy):
pass
class RouterAltitudeArea:
def __init__(self, geometry, altitude, altitude2, point1, point2):
self.geometry = geometry
self.altitude = altitude
self.altitude2 = altitude2
self.point1 = point1
self.point2 = point2
@cached_property
def geometry_prep(self):
return prepared.prep(self.geometry)
def get_altitude(self, point):
# noinspection PyTypeChecker,PyCallByClass
return AltitudeArea.get_altitudes(self, (point.x, point.y))[0]
def __getstate__(self):
result = self.__dict__.copy()
result.pop('geometry_prep', None)
return result
class RouterNode:
def __init__(self, pk, x, y, space, altitude=None, areas=None):
self.pk = pk
self.x = x
self.y = y
self.space = space
self.altitude = altitude
self.areas = areas if areas else set()
@classmethod
def from_graph_node(cls, node):
return cls(node.pk, node.geometry.x, node.geometry.y, node.space_id)
@cached_property
def point(self):
return Point(self.x, self.y)
@cached_property
def xy(self):
return np.array((self.x, self.y))
class RouterEdge:
def __init__(self, from_node, to_node, waytype, rise=None, distance=None):
self.from_node = from_node
self.to_node = to_node
self.waytype = waytype
self.rise = rise if rise is not None else (self.to_node.altitude - self.from_node.altitude)
self.distance = distance if distance is not None else np.linalg.norm(to_node.xy - from_node.xy)
class RouterWayType:
def __init__(self, waytype):
self.waytype = waytype
self.upwards_indices = deque()
self.nonupwards_indices = deque()

172
src/c3nav/routing/routesegments.py
View file

@ -1,172 +0,0 @@
from abc import ABC, abstractmethod
from django.utils.functional import cached_property
from c3nav.routing.connection import GraphConnection
from c3nav.routing.point import GraphPoint
from c3nav.routing.route import Route
class RouteSegment(ABC):
def __init__(self, routers, router, from_point, to_point):
"""
:param router: a Router (RoomRouter, GraphRouter, …)
:param from_point: in-router index of first point
:param to_point: in-router index of last point
"""
self.routers = routers
self.router = router
self.from_point = int(from_point)
self.to_point = int(to_point)
def as_route(self):
return SegmentRoute([self])
def _get_points(self):
points = [self.to_point]
first = self.from_point
current = self.to_point
while current != first:
current = self.router.predecessors[first, current]
points.append(current)
return tuple(reversed(points))
@abstractmethod
def get_connections(self):
pass
@cached_property
def distance(self):
return self.router.shortest_paths[self.from_point, self.to_point]
class RoomRouteSegment(RouteSegment):
def __init__(self, room, routers, from_point, to_point):
"""
Route segment within a Room
:param room: GraphRoom
"""
super().__init__(routers, routers[room], from_point, to_point)
self.room = room
self.global_from_point = room.points[from_point]
self.global_to_point = room.points[to_point]
def get_connections(self):
points = self._get_points()
return tuple(self.room.get_connection(from_point, to_point)
for from_point, to_point in zip(points[:-1], points[1:]))
def __repr__(self):
return ('<RoomRouteSegment in %r from points %d to %d with distance %f>' %
(self.room, self.from_point, self.to_point, self.distance))
class LevelRouteSegment(RouteSegment):
def __init__(self, level, routers, from_point, to_point):
"""
Route segment within a Level (from room transfer point to room transfer point)
:param level: GraphLevel
"""
super().__init__(routers, routers[level], from_point, to_point)
self.level = level
self.global_from_point = level.room_transfer_points[from_point]
self.global_to_point = level.room_transfer_points[to_point]
def split(self):
segments = []
points = self._get_points()
for from_point, to_point in zip(points[:-1], points[1:]):
room = self.level.rooms[self.router.room_transfers[from_point, to_point]]
global_from_point = self.level.room_transfer_points[from_point]
global_to_point = self.level.room_transfer_points[to_point]
segments.append(RoomRouteSegment(room, self.routers,
from_point=room.points.index(global_from_point),
to_point=room.points.index(global_to_point)))
return tuple(segments)
def get_connections(self):
return sum((segment.get_connections() for segment in self.split()), ())
def __repr__(self):
return ('<LevelRouteSegment in %r from points %d to %d with distance %f>' %
(self.level, self.from_point, self.to_point, self.distance))
class GraphRouteSegment(RouteSegment):
def __init__(self, graph, routers, from_point, to_point):
"""
Route segment within a Graph (from level transfer point to level transfer point)
:param graph: Graph
"""
super().__init__(routers, routers[graph], from_point, to_point)
self.graph = graph
self.global_from_point = graph.level_transfer_points[from_point]
self.global_to_point = graph.level_transfer_points[to_point]
def split(self):
segments = []
points = self._get_points()
for from_point, to_point in zip(points[:-1], points[1:]):
level = tuple(self.graph.levels.values())[self.router.level_transfers[from_point, to_point]]
global_from_point = self.graph.level_transfer_points[from_point]
global_to_point = self.graph.level_transfer_points[to_point]
segments.append(LevelRouteSegment(level, self.routers,
from_point=level.room_transfer_points.index(global_from_point),
to_point=level.room_transfer_points.index(global_to_point)))
return tuple(segments)
def get_connections(self):
return sum((segment.get_connections() for segment in self.split()), ())
def __repr__(self):
return ('<GraphRouteSegment in %r from points %d to %d with distance %f>' %
(self.graph, self.from_point, self.to_point, self.distance))
class SegmentRoute:
def __init__(self, segments, distance=None):
self.segments = sum(((item.segments if isinstance(item, SegmentRoute) else (item,))
for item in segments if item.from_point != item.to_point), ())
self.distance = sum(segment.distance for segment in self.segments)
self.from_point = segments[0].global_from_point
self.to_point = segments[-1].global_to_point
self.global_from_point = self.from_point
self.global_to_point = self.to_point
def __repr__(self):
return ('<SegmentedRoute (\n %s\n) distance=%f>' %
('\n '.join(repr(segment) for segment in self.segments), self.distance))
def rawsplit(self):
return sum((segment.get_connections() for segment in self.segments), ())
def split(self):
return Route(self.rawsplit())
class SegmentRouteWrapper:
def __init__(self, segmentroute: SegmentRoute, orig_point, dest_point, orig_ctype, dest_ctype):
self.segmentroute = segmentroute
self.orig_point = orig_point
self.dest_point = dest_point
self.orig_ctype = orig_ctype
self.dest_ctype = dest_ctype
def __repr__(self):
return ('<SegmentedRouteWrapper %s, add_orig_point=%s, add_dest_point=%s>' %
(repr(self.segmentroute), repr(self.orig_point), repr(self.dest_point)))
def split(self):
connections = self.segmentroute.rawsplit()
if self.orig_point:
first_point = connections[0].from_point
orig_point = GraphPoint(self.orig_point.x, self.orig_point.y, first_point.room)
connections = (GraphConnection(orig_point, first_point, ctype=self.orig_ctype),) + connections
if self.dest_point:
last_point = connections[-1].to_point
dest_point = GraphPoint(self.dest_point.x, self.dest_point.y, last_point.room)
connections = connections + (GraphConnection(last_point, dest_point, ctype=self.dest_ctype), )
return Route(connections)