team-3/src/c3nav/routing/graph.py

import os
import pickle
from collections import OrderedDict, namedtuple

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 import Level
from c3nav.mapdata.models.geometry import LevelConnector
from c3nav.mapdata.models.locations import AreaLocation, Location, LocationGroup, PointLocation
from c3nav.routing.level import GraphLevel
from c3nav.routing.point import GraphPoint


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

    # Building the Graph
    def build(self):
        self._built_level_transfer_points = []
        self._built_levelconnector_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()

        # 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 connect_levelconnectors(self):
        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)

    # 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):
        level_routers = {}
        room_routers = {}

        empty_distances = np.empty(shape=(len(self.level_transfer_points),) * 2, dtype=np.float16)
        empty_distances[:] = np.inf

        sparse_distances = empty_distances.copy()

        sparse_levels = np.zeros(shape=(len(self.level_transfer_points),) * 2, dtype=np.int16)
        sparse_levels[:] = -1

        for i, level in enumerate(self.levels.values()):
            router, add_room_routers = level.build_routers()
            level_routers[level] = router
            room_routers.update(add_room_routers)

            level_distances = empty_distances.copy()
            in_level_i = np.array(tuple(level.room_transfer_points.index(point)
                                        for point in level.level_transfer_points))
            in_graph_i = np.array(tuple(self.level_transfer_points.index(point)
                                        for point in level.level_transfer_points))
            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.transpose()] = level_distances[better.transpose()]
            sparse_levels[better.transpose()] = i

        g_sparse = csgraph_from_dense(sparse_distances, null_value=np.inf)
        shortest_paths, predecessors = shortest_path(g_sparse, return_predecessors=True)
        return GraphRouter(shortest_paths, predecessors), level_routers, room_routers

    def get_location_points(self, location: Location):
        if isinstance(location, PointLocation):
            return 'bla'
        elif isinstance(location, AreaLocation):
            return self.levels[location.level.name].arealocation_points[location.name]
        elif isinstance(location, LocationGroup):
            return np.hstack(tuple(self.get_location_points(area) for area in location.locationareas))

    def _get_points_by_i(self, points):
        return tuple(self.points[i] for i in points)

    def _get_index_of_allowed_points(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):
        orig_points_i = set(self.get_location_points(origin))
        dest_points_i = set(self.get_location_points(destination))

        orig_points = self._get_points_by_i(orig_points_i)
        dest_points = self._get_points_by_i(dest_points_i)

        best_route_distance = float('inf')
        best_route = None

        # get routers
        graph_router, level_routers, room_routers = self.build_routers()

        # 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

        # get origin points for each room (points as point index within room)
        orig_room_points = {room: self._get_index_of_allowed_points(room.points, orig_points_i) for room in orig_rooms}
        dest_room_points = {room: self._get_index_of_allowed_points(room.points, dest_points_i) for room in dest_rooms}

        # if the points have common rooms, search for routes within those rooms
        if common_rooms:
            for room in common_rooms:
                shortest_paths = room_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
                best_route = ('room', room, np.argwhere(shortest_paths == distance)[0])
                best_route_distance = distance

        # get reachable room transfer points and their distance
        # as a dictionary: global transfer point index => (global index of closest location point, distance)
        orig_room_transfers = self._room_transfers(orig_rooms, orig_room_points, room_routers, mode='orig')
        dest_room_transfers = self._room_transfers(dest_rooms, dest_room_points, room_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_level_points = {level: self._get_index_of_allowed_points(level.room_transfer_points, orig_room_transfers)
                             for level in orig_levels}
        dest_level_points = {level: self._get_index_of_allowed_points(level.room_transfer_points, dest_room_transfers)
                             for level in dest_levels}

        if common_levels:
            for level in common_levels:
                o_points = orig_level_points[level]
                d_points = dest_level_points[level]
                shortest_paths = level_routers[level].shortest_paths[o_points[:, None], d_points]
                shortest_paths += np.array(tuple(orig_room_transfers[level.room_transfer_points[in_level_i]][1]
                                                 for in_level_i in o_points))[:, None]
                shortest_paths += np.array(tuple(dest_room_transfers[level.room_transfer_points[in_level_i]][1]
                                                 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
                best_route = ('level', level, np.argwhere(shortest_paths == distance)[0])
                best_route_distance = distance

        return best_route

    def _room_transfers(self, rooms, room_points, room_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))

            points = room_points[room]
            if mode == 'orig':
                shortest_paths = room_routers[room].shortest_paths[points[:, None], room_transfer_points]
            else:
                shortest_paths = room_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]
                location_i, transfer_i = (from_i, to_i) if mode == 'orig' else (to_i, from_i)
                location_i = room.points[points[location_i]]
                transfer_i = room.points[room_transfer_points[transfer_i]]
                if transfer_i not in room_transfers or room_transfers[transfer_i][1] < distance:
                    room_transfers[transfer_i] = (location_i, distance)

        return room_transfers


GraphRouter = namedtuple('GraphRouter', ('shortest_paths', 'predecessors', ))