Merge pull request #75 from valhalla/kk_shorter

take the shortest path even if its basically a crap shoot

Author: kevinkreiser

src/thor/pathalgorithm.cc

@@ -56,7 +56,8 @@ namespace thor {
 PathAlgorithm::PathAlgorithm()
     : edgelabel_index_(0),
       adjacencylist_(nullptr),
-      edgestatus_(nullptr) {
+      edgestatus_(nullptr),
+      best_destination_{kInvalidLabel, std::numeric_limits<float>::max()}{
   edgelabels_.reserve(kInitialEdgeLabelCount);
 }
 
@@ -70,6 +71,7 @@ void PathAlgorithm::Clear() {
   // Set the edge label index back to 0
   edgelabel_index_ = 0;
   edgelabels_.clear();
+  best_destination_ = std::make_pair(kInvalidLabel, std::numeric_limits<float>::max());
 
   // Clear elements from the adjacency list
   if(adjacencylist_ != nullptr) {
@@ -123,7 +125,7 @@ std::vector<GraphId> PathAlgorithm::GetBestPath(const PathLocation& origin,
 
   // Initialize the origin and destination locations
   SetOrigin(graphreader, origin, costing, loop_edge);
-  SetDestination(dest);
+  SetDestination(graphreader, dest, costing);
 
   // Counts of transitions to upper levels (TEST - TODO better design!)
   uint32_t upto1count = 0;
@@ -146,6 +148,10 @@ std::vector<GraphId> PathAlgorithm::GetBestPath(const PathLocation& origin,
     // error codes and return the appropriate one here
     next_label_index = adjacencylist_->Remove(edgelabels_);
     if (next_label_index == kInvalidLabel) {
+      // If we had a destination but we were waiting on other possible ones
+      if(best_destination_.first != kInvalidLabel)
+        return FormPath(best_destination_.first, graphreader, loop_edge);
+      // We didn't find any destination edge
       LOG_ERROR("Route failed after iterations = " + std::to_string(edgelabel_index_));
       throw std::runtime_error("No path could be found for input");
     }
@@ -156,8 +162,8 @@ std::vector<GraphId> PathAlgorithm::GetBestPath(const PathLocation& origin,
     edgestatus_->Set(nextlabel.edgeid(), kPermanent);
 
     // Check for completion. Form path and return if complete.
-    if (IsComplete(nextlabel)) {
-      return FormPath(next_label_index, graphreader, loop_edge);
+    if (IsComplete(next_label_index)) {
+      return FormPath(best_destination_.first, graphreader, loop_edge);
     }
 
     // TODO - do we need to terminate fruitless searches?
@@ -283,7 +289,7 @@ std::vector<GraphId> PathAlgorithm::GetBestPath(const PathLocation& origin,
 }
 
 // Add an edge at the origin to the adjacency list
-void PathAlgorithm::SetOrigin(baldr::GraphReader& graphreader,
+void PathAlgorithm::SetOrigin(GraphReader& graphreader,
           const PathLocation& origin, const std::shared_ptr<DynamicCost>& costing, const GraphId& loop_edge_id) {
   // Get sort heuristic based on distance from origin to destination
   float dist = astarheuristic_.GetDistance(origin.vertex());
@@ -328,21 +334,37 @@ void PathAlgorithm::SetOrigin(baldr::GraphReader& graphreader,
 }
 
 // Add a destination edge
-void PathAlgorithm::SetDestination(const PathLocation& dest) {
-  // TODO - add partial distances
+void PathAlgorithm::SetDestination(GraphReader& graphreader, const PathLocation& dest, const std::shared_ptr<DynamicCost>& costing) {
+  // For each edge
   for (const auto& edge : dest.edges()) {
-    destinations_[edge.id] = edge.dist;
+    // Keep the id and the cost to traverse the partial distance
+    const GraphTile* tile = graphreader.GetGraphTile(edge.id);
+    destinations_[edge.id] = costing->Get(tile->directededge(edge.id)) * edge.dist;
   }
 }
 
 // Test is the shortest path has been found.
-bool PathAlgorithm::IsComplete(const EdgeLabel& edge_label) {
-  // TODO - if destination is along an edge and the edge allows
-  // travel in both directions we need to make sure both directions
-  // are found or some further cost is encountered to rule out the
-  // other direction
-  const auto& p = destinations_.find(edge_label.edgeid());
-  return p != destinations_.end();
+bool PathAlgorithm::IsComplete(const uint32_t edge_label_index) {
+  //grab the label
+  const EdgeLabel& edge_label = edgelabels_[edge_label_index];
+  //if we've already found a destination and the search's current edge is more costly to get to, we are done
+  if(best_destination_.first != kInvalidLabel && edge_label.truecost() > best_destination_.second)
+    return true;
+  //check if its a destination
+  auto p = destinations_.find(edge_label.edgeid());
+  //it is indeed one of the possible destination edges
+  if(p != destinations_.end()) {
+    //if we didnt have another destination yet or this one is better
+    auto cost = edge_label.truecost() + p->second;
+    if(best_destination_.first == kInvalidLabel || cost < best_destination_.second){
+      best_destination_.first = edge_label_index;
+      best_destination_.second = cost;
+    }
+    destinations_.erase(p);
+    //if we've found all of the destinations we are done looking
+    return destinations_.size() == 0;
+  }
+  return false;
 }
 
 // Form the path from the adjacency list.

src/thor/trippathbuilder.cc

@@ -119,7 +119,7 @@ TripPath TripPathBuilder::Build(GraphReader& graphreader,
   // If the path was only one edge we have a special case
   if(pathedges.size() == 1) {
     if(end_pct < start_pct)
-       throw std::runtime_error("Generated reverse trivial path, this is a bug and we are working on it");
+       throw std::runtime_error("Generated reverse trivial path, report this bug!");
     const auto tile = graphreader.GetGraphTile(pathedges.front());
     const auto edge = tile->directededge(pathedges.front());
 

valhalla/thor/pathalgorithm.h

@@ -4,6 +4,7 @@
 #include <vector>
 #include <map>
 #include <unordered_map>
+#include <utility>
 
 #include <valhalla/baldr/graphid.h>
 #include <valhalla/baldr/graphreader.h>
@@ -64,9 +65,12 @@ class PathAlgorithm {
   // to search to find an entry that is already in the adjacency list
   std::unordered_map<baldr::GraphId, uint32_t> adjlistedges_;
 
-  // Destinations
+  // Destinations, id and cost
   std::unordered_map<baldr::GraphId, float> destinations_;
 
+  // Destination that was last found with its true cost + partial cost
+  std::pair<uint32_t, float> best_destination_;
+
   /**
    * Initialize
    */
@@ -82,14 +86,16 @@ class PathAlgorithm {
   /**
    * Set the destination edge(s).
    */
-  void SetDestination(const baldr::PathLocation& dest);
+  void SetDestination(baldr::GraphReader& graphreader, const baldr::PathLocation& dest,
+     const std::shared_ptr<DynamicCost>& costing);
 
   /**
-   * Test if the shortest path is found.
+   * Return a valid edge id if we've found the destination edge
    *
-   * @param edge_label        edgelabel to be tested for destination
+   * @param edge_label_index     edge label to be tested for destination
+   * @return bool                true if we've found the destination
    */
-  bool IsComplete(const EdgeLabel& edge_label);
+  bool IsComplete(const uint32_t edge_label_index);
 
   /**
    * Form the path from the adjacency list.