debug output

include/osr/routing/dijkstra.h

@@ -9,6 +9,8 @@ namespace osr {
 
 struct sharing_data;
 
+constexpr auto const kDebug = false;
+
 template <typename Profile>
 struct dijkstra {
   using profile_t = Profile;
@@ -28,9 +30,14 @@ struct dijkstra {
     cost_.clear();
   }
 
-  void add_start(label const l) {
+  void add_start(ways const& w, label const l) {
     if (cost_[l.get_node().get_key()].update(l, l.get_node(), l.cost(),
                                              node::invalid())) {
+      if constexpr (kDebug) {
+        std::cout << "START ";
+        l.get_node().print(std::cout, w);
+        std::cout << "\n";
+      }
       pq_.push(l);
     }
   }
@@ -41,7 +48,8 @@ struct dijkstra {
   }
 
   template <direction SearchDir, bool WithBlocked>
-  void run(ways::routing const& r,
+  void run(ways const& w,
+           ways::routing const& r,
            cost_t const max,
            bitvec<node_idx_t> const* blocked,
            sharing_data const* sharing) {
@@ -51,36 +59,56 @@ struct dijkstra {
         continue;
       }
 
+      if constexpr (kDebug) {
+        std::cout << "EXTRACT ";
+        l.get_node().print(std::cout, w);
+        std::cout << "\n";
+      }
+
       auto const curr = l.get_node();
       Profile::template adjacent<SearchDir, WithBlocked>(
           r, curr, blocked, sharing,
           [&](node const neighbor, std::uint32_t const cost, distance_t,
               way_idx_t const way, std::uint16_t, std::uint16_t) {
+            if constexpr (kDebug) {
+              std::cout << "  NEIGHBOR ";
+              neighbor.print(std::cout, w);
+            }
+
             auto const total = l.cost() + cost;
             if (total < max &&
                 cost_[neighbor.get_key()].update(
                     l, neighbor, static_cast<cost_t>(total), curr)) {
               auto next = label{neighbor, static_cast<cost_t>(total)};
               next.track(l, r, way, neighbor.get_node());
               pq_.push(std::move(next));
+
+              if constexpr (kDebug) {
+                std::cout << " -> PUSH\n";
+              }
+            } else {
+              if constexpr (kDebug) {
+                std::cout << " -> DOMINATED\n";
+              }
             }
           });
     }
   }
 
-  void run(ways::routing const& r,
+  void run(ways const& w,
+           ways::routing const& r,
            cost_t const max,
            bitvec<node_idx_t> const* blocked,
            sharing_data const* sharing,
            direction const dir) {
     if (blocked == nullptr) {
       dir == direction::kForward
-          ? run<direction::kForward, false>(r, max, blocked, sharing)
-          : run<direction::kBackward, false>(r, max, blocked, sharing);
+          ? run<direction::kForward, false>(w, r, max, blocked, sharing)
+          : run<direction::kBackward, false>(w, r, max, blocked, sharing);
     } else {
       dir == direction::kForward
-          ? run<direction::kForward, true>(r, max, blocked, sharing)
-          : run<direction::kBackward, true>(r, max, blocked, sharing);
+          ? run<direction::kForward, true>(w, r, max, blocked, sharing)
+          : run<direction::kBackward, true>(w, r, max, blocked, sharing);
     }
   }
 

include/osr/routing/profiles/bike_sharing.h

@@ -107,7 +107,10 @@ struct bike_sharing {
     }
 
     std::ostream& print(std::ostream& out, ways const& w) const {
-      return out << "(node=" << w.node_to_osm_[n_]
+      return out << "(node="
+                 << (n_ >= w.n_nodes() ? osm_node_idx_t{to_idx(n_)}
+                                       : w.node_to_osm_[n_])
+                 << (n_ >= w.n_nodes() ? "*" : "")
                  << ", level=" << lvl_.to_float()
                  << ", type=" << node_type_to_str(type_) << ")";
     }
@@ -252,6 +255,7 @@ struct bike_sharing {
     footp::resolve_all(w, n, lvl, [&](footp::node const neighbor) {
       f(to_node(neighbor, node_type::kInitialFoot));
       f(to_node(neighbor, node_type::kTrailingFoot));
+      f(to_node(neighbor, node_type::kBike));
     });
   }
 

src/route.cc

@@ -301,15 +301,15 @@ std::optional<path> route(ways const& w,
       if (nc->valid() && nc->cost_ < max) {
         Profile::resolve_start_node(
             *w.r_, start.way_, nc->node_, from.lvl_, dir,
-            [&](auto const node) { d.add_start({node, nc->cost_}); });
+            [&](auto const node) { d.add_start(w, {node, nc->cost_}); });
       }
     }
 
     if (d.pq_.empty()) {
       continue;
     }
 
-    d.run(*w.r_, max, blocked, sharing, dir);
+    d.run(w, *w.r_, max, blocked, sharing, dir);
 
     auto const c = best_candidate(w, d, to.lvl_, to_match, max, dir);
     if (c.has_value()) {
@@ -350,12 +350,12 @@ std::vector<std::optional<path>> route(
             *w.r_, start.way_, nc->node_, from.lvl_, dir, [&](auto const node) {
               auto label = typename Profile::label{node, nc->cost_};
               label.track(label, *w.r_, start.way_, node.get_node());
-              d.add_start(label);
+              d.add_start(w, label);
             });
       }
     }
 
-    d.run(*w.r_, max, blocked, sharing, dir);
+    d.run(w, *w.r_, max, blocked, sharing, dir);
 
     auto found = 0U;
     for (auto const [m, t, r] : utl::zip(to_match, to, result)) {