From dd3a885749334ab50eabc30b389450c7c35a9fdf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Laura=20Kl=C3=BCnder?= Date: Mon, 27 Nov 2017 02:16:20 +0100 Subject: [PATCH] start implementing routing by creating new router --- src/c3nav/mapdata/models/geometry/level.py | 2 +- src/c3nav/routing/area.py | 127 ---- src/c3nav/routing/connection.py | 20 - src/c3nav/routing/graph.py | 543 ------------------ src/c3nav/routing/level.py | 430 -------------- src/c3nav/routing/management/__init__.py | 0 .../routing/management/commands/__init__.py | 0 .../routing/management/commands/buildgraph.py | 24 - .../routing/management/commands/drawgraph.py | 18 - .../routing/management/commands/testgraph.py | 22 - src/c3nav/routing/point.py | 46 -- src/c3nav/routing/room.py | 332 ----------- src/c3nav/routing/router.py | 218 +++++++ src/c3nav/routing/routesegments.py | 172 ------ 14 files changed, 219 insertions(+), 1735 deletions(-) delete mode 100644 src/c3nav/routing/area.py delete mode 100644 src/c3nav/routing/connection.py delete mode 100644 src/c3nav/routing/graph.py delete mode 100644 src/c3nav/routing/level.py delete mode 100644 src/c3nav/routing/management/__init__.py delete mode 100644 src/c3nav/routing/management/commands/__init__.py delete mode 100644 src/c3nav/routing/management/commands/buildgraph.py delete mode 100644 src/c3nav/routing/management/commands/drawgraph.py delete mode 100644 src/c3nav/routing/management/commands/testgraph.py delete mode 100644 src/c3nav/routing/point.py delete mode 100644 src/c3nav/routing/room.py create mode 100644 src/c3nav/routing/router.py delete mode 100644 src/c3nav/routing/routesegments.py diff --git a/src/c3nav/mapdata/models/geometry/level.py b/src/c3nav/mapdata/models/geometry/level.py index 6abe7302..3cca521a 100644 --- a/src/c3nav/mapdata/models/geometry/level.py +++ b/src/c3nav/mapdata/models/geometry/level.py @@ -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): diff --git a/src/c3nav/routing/area.py b/src/c3nav/routing/area.py deleted file mode 100644 index 321847e7..00000000 --- a/src/c3nav/routing/area.py +++ /dev/null @@ -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 diff --git a/src/c3nav/routing/connection.py b/src/c3nav/routing/connection.py deleted file mode 100644 index e4158bd2..00000000 --- a/src/c3nav/routing/connection.py +++ /dev/null @@ -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 ('' % - (self.from_point, self.to_point, self.distance, self.ctype)) diff --git a/src/c3nav/routing/graph.py b/src/c3nav/routing/graph.py deleted file mode 100644 index e0607adc..00000000 --- a/src/c3nav/routing/graph.py +++ /dev/null @@ -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', )) diff --git a/src/c3nav/routing/level.py b/src/c3nav/routing/level.py deleted file mode 100644 index 7606fb1b..00000000 --- a/src/c3nav/routing/level.py +++ /dev/null @@ -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')) diff --git a/src/c3nav/routing/management/__init__.py b/src/c3nav/routing/management/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/c3nav/routing/management/commands/__init__.py b/src/c3nav/routing/management/commands/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/c3nav/routing/management/commands/buildgraph.py b/src/c3nav/routing/management/commands/buildgraph.py deleted file mode 100644 index 63ac017d..00000000 --- a/src/c3nav/routing/management/commands/buildgraph.py +++ /dev/null @@ -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)) diff --git a/src/c3nav/routing/management/commands/drawgraph.py b/src/c3nav/routing/management/commands/drawgraph.py deleted file mode 100644 index eef67bf1..00000000 --- a/src/c3nav/routing/management/commands/drawgraph.py +++ /dev/null @@ -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']) diff --git a/src/c3nav/routing/management/commands/testgraph.py b/src/c3nav/routing/management/commands/testgraph.py deleted file mode 100644 index 84120783..00000000 --- a/src/c3nav/routing/management/commands/testgraph.py +++ /dev/null @@ -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)) diff --git a/src/c3nav/routing/point.py b/src/c3nav/routing/point.py deleted file mode 100644 index 0c706e97..00000000 --- a/src/c3nav/routing/point.py +++ /dev/null @@ -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 '' % (self.x, self.y, (id(self.room) if self.room else None)) diff --git a/src/c3nav/routing/room.py b/src/c3nav/routing/room.py deleted file mode 100644 index c3452957..00000000 --- a/src/c3nav/routing/room.py +++ /dev/null @@ -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', )) diff --git a/src/c3nav/routing/router.py b/src/c3nav/routing/router.py new file mode 100644 index 00000000..9471928f --- /dev/null +++ b/src/c3nav/routing/router.py @@ -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() diff --git a/src/c3nav/routing/routesegments.py b/src/c3nav/routing/routesegments.py deleted file mode 100644 index 738cf033..00000000 --- a/src/c3nav/routing/routesegments.py +++ /dev/null @@ -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 ('' % - (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 ('' % - (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 ('' % - (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 ('' % - ('\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 ('' % - (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)