include additional distance at origin and destination

src/c3nav/routing/route.py

@@ -17,7 +17,7 @@ def describe_location(location, locations):
 
 class Route:
     def __init__(self, router, origin, destination, path_nodes, options,
-                 origin_addition, destination_addition):
+                 origin_addition, destination_addition, origin_xyz, destination_xyz):
         self.router = router
         self.origin = origin
         self.destination = destination
@@ -25,6 +25,8 @@ class Route:
         self.options = options
         self.origin_addition = origin_addition
         self.destination_addition = destination_addition
+        self.origin_xyz = origin_xyz
+        self.destination_xyz = destination_xyz
 
     def serialize(self, locations):
         nodes = [[node, None] for node in self.path_nodes]
@@ -34,12 +36,22 @@ class Route:
         if self.destination_addition and any(self.destination_addition):
             nodes.append(self.destination_addition)
 
+        if self.origin_xyz is not None:
+            origin_distance = np.linalg.norm(self.router.nodes[nodes[0][0]].xyz - self.origin_xyz)
+        else:
+            origin_distance = 0
+
+        if self.destination_xyz is not None:
+            destination_distance = np.linalg.norm(self.router.nodes[nodes[-1][0]].xyz - self.destination_xyz)
+        else:
+            destination_distance = 0
+
         items = deque()
         last_node = None
         last_item = None
-        distance = 0
-        duration = 0
         walk_factor = self.options.walk_factor
+        distance = origin_distance
+        duration = origin_distance * walk_factor
         for i, (node, edge) in enumerate(nodes):
             if edge is None:
                 edge = self.router.edges[last_node, node] if last_node else None
@@ -52,6 +64,9 @@ class Route:
             last_item = item
             last_node = node
 
+        distance += destination_distance
+        duration += destination_distance * walk_factor
+
         # descriptions for waytypes
         next_item = None
         for item in reversed(items):
@@ -76,7 +91,7 @@ class Route:
             next_item = item
 
         # add description for last space
-        remaining_distance = 0
+        remaining_distance = destination_distance
         for item in reversed(items):
             if item.descriptions:
                 break

src/c3nav/routing/router.py

@@ -157,6 +157,7 @@ class Router:
 
                     poi = RouterPoint(poi)
                     altitudearea = space.altitudearea_for_point(poi.geometry)
+                    poi.altitude = altitudearea.get_altitude(poi.geometry)
                     poi_nodes = altitudearea.nodes_for_point(poi.geometry, all_nodes=nodes)
                     poi.nodes = set(i for i in poi_nodes.keys())
                     poi.nodes_addition = poi_nodes
@@ -274,6 +275,7 @@ class Router:
             location = RouterPoint(location)
             space = self.space_for_point(location.level.pk, point, restrictions)
             altitudearea = space.altitudearea_for_point(point)
+            location.altitude = altitudearea.get_altitude(point)
             location_nodes = altitudearea.nodes_for_point(point, all_nodes=self.nodes)
             location.nodes = set(i for i in location_nodes.keys())
             location.nodes_addition = location_nodes
@@ -397,8 +399,12 @@ class Router:
         origin_addition = origin.nodes_addition.get(origin_node)
         destination_addition = destination.nodes_addition.get(destination_node)
 
+        # get additional distance at origin and destination
+        origin_xyz = origin.xyz if isinstance(origin, RouterPoint) else None
+        destination_xyz = destination.xyz if isinstance(destination, RouterPoint) else None
+
         return Route(self, origin, destination, path_nodes, options,
-                     origin_addition, destination_addition)
+                     origin_addition, destination_addition, origin_xyz, destination_xyz)
 
 
 CustomLocationDescription = namedtuple('CustomLocationDescription', ('space', 'altitude'))
@@ -449,7 +455,13 @@ class RouterArea(BaseRouterProxy):
 
 
 class RouterPoint(BaseRouterProxy):
-    pass
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.altitude = None
+
+    @cached_property
+    def xyz(self):
+        return np.array((self.x, self.y, self.altitude))
 
 
 class RouterAltitudeArea: