A* + BiDir A*

CMakeLists.txt

@@ -107,7 +107,10 @@ endif ()
 
 # --- LIB ---
 file(GLOB_RECURSE osr-src src/*.cc)
-add_library(osr ${osr-src})
+add_library(osr ${osr-src}
+        include/osr/routing/a_star.h
+        include/osr/routing/bidirectional.h
+        include/osr/routing/algorithms.h)
 target_include_directories(osr PUBLIC include)
 target_compile_features(osr PUBLIC cxx_std_23)
 target_compile_options(osr PRIVATE ${osr-compile-options})
@@ -130,7 +133,9 @@ add_executable(osr-extract exe/extract.cc)
 target_link_libraries(osr-extract osr)
 
 add_executable(osr-benchmark exe/benchmark.cc)
+add_executable(osr-test-aStar exe/test.cc)
 target_link_libraries(osr-benchmark osr)
+target_link_libraries(osr-test-aStar osr)
 
 file(GLOB_RECURSE osr-backend-src exe/backend/*.cc)
 add_executable(osr-backend ${osr-backend-src})

exe/backend/src/http_server.cc

@@ -103,11 +103,20 @@ struct http_server::impl {
                : to_profile(profile_it->value().as_string());
   }
 
+  static routing_algorithm get_routing_algorithm_from_request(
+      boost::json::object const& q) {
+    auto const routing_it = q.find("routing");
+    return routing_it == q.end() || !routing_it->value().is_string()
+               ? routing_algorithm::kDijkstra
+               : to_algorithm(routing_it->value().as_string());
+  }
+
   void handle_route(web_server::http_req_t const& req,
                     web_server::http_res_cb_t const& cb) {
     auto const q = boost::json::parse(req.body()).as_object();
     auto const profile = get_search_profile_from_request(q);
     auto const direction_it = q.find("direction");
+    auto const routing_algo = get_routing_algorithm_from_request(q);
     auto const dir = to_direction(direction_it == q.end() ||
                                           !direction_it->value().is_string()
                                       ? to_str(direction::kForward)
@@ -117,8 +126,8 @@ struct http_server::impl {
     auto const max_it = q.find("max");
     auto const max = static_cast<cost_t>(
         max_it == q.end() ? 3600 : max_it->value().as_int64());
-    auto const p = route(w_, l_, profile, from, to, max, dir, 100, nullptr,
-                         nullptr, elevations_);
+    auto const p = route(w_, l_, profile, routing_algo, from, to, max, dir, 100,
+                         nullptr, nullptr, elevations_);
     if (!p.has_value()) {
       cb(json_response(req, "could not find a valid path",
                        http::status::not_found));

exe/test.cc

@@ -0,0 +1,145 @@
+#include <filesystem>
+#include <thread>
+#include <vector>
+
+#include "boost/asio/executor_work_guard.hpp"
+#include "boost/asio/io_context.hpp"
+
+#include "fmt/core.h"
+#include "fmt/std.h"
+
+#include "conf/options_parser.h"
+
+#include "utl/timer.h"
+
+#include "osr/lookup.h"
+#include "osr/routing/a_star.h"
+#include "osr/routing/dijkstra.h"
+#include "osr/routing/profiles/car.h"
+#include "osr/routing/route.h"
+#include "osr/ways.h"
+
+namespace fs = std::filesystem;
+using namespace osr;
+
+class settings : public conf::configuration {
+public:
+  explicit settings() : configuration("Options") {
+    param(data_dir_, "data,d", "Data directory");
+    param(threads_, "threads,t", "Number of routing threads");
+    param(n_queries_, "n", "Number of queries");
+    param(max_dist_, "r", "Radius");
+  }
+
+  fs::path data_dir_{"osr"};
+  unsigned n_queries_{50};
+  unsigned max_dist_{1200};
+  unsigned threads_{std::thread::hardware_concurrency()};
+};
+
+std::vector<int> generate_random_vector(int upperBound, int n) {
+  std::srand(std::time(0));
+  int lowerBound = 0;
+  std::vector<int> random_numbers(n);
+  for (int i = 0; i < n; ++i) {
+    random_numbers[i] =
+        lowerBound + std::rand() % (upperBound - lowerBound + 1);
+  }
+  return random_numbers;
+}
+
+int main(int argc, char const* argv[]) {
+  auto opt = settings{};
+  auto parser = conf::options_parser({&opt});
+  parser.read_command_line_args(argc, argv);
+
+  if (parser.help()) {
+    parser.print_help(std::cout);
+    return 0;
+  } else if (parser.version()) {
+    return 0;
+  }
+
+  parser.read_configuration_file();
+  parser.print_unrecognized(std::cout);
+  parser.print_used(std::cout);
+
+  if (!fs::is_directory(opt.data_dir_)) {
+    fmt::println("directory not found: {}", opt.data_dir_);
+    return 1;
+  }
+
+  auto const w = ways{opt.data_dir_, cista::mmap::protection::READ};
+
+  auto timer = utl::scoped_timer{"timer"};
+  auto i = std::atomic_size_t{0U};
+  auto d = dijkstra<car>{};
+  auto h = cista::BASE_HASH;
+  auto n = 0U;
+  auto const elevations = elevation_storage::try_open(opt.data_dir_);
+  auto const start =
+      node_idx_t{cista::hash_combine(h, ++n, i.load()) % w.n_nodes()};
+  auto start_lf = car::label{car::node{start, 0, direction::kForward}, 0U};
+  auto start_lb = car::label{car::node{start, 0, direction::kBackward}, 0U};
+  d.reset(opt.max_dist_);
+  d.add_start(w, start_lf);
+  d.add_start(w, start_lb);
+  d.run<direction::kForward, false>(w, *w.r_, opt.max_dist_, nullptr, nullptr,
+                                    elevations.get());
+
+  std::vector<int> random_numbers =
+      generate_random_vector(d.cost_.size(), opt.n_queries_);
+
+  if (n > d.cost_.size()) {
+    std::cerr << "n is greater than the size of the cost array \n";
+  }
+
+  auto timer_a = utl::scoped_timer{"timer"};
+  auto threads = std::vector<std::thread>(std::max(1U, opt.threads_));
+  for (auto& t : threads) {
+    t = std::thread([&]() {
+      while (i.fetch_add(1U) < opt.n_queries_) {
+        auto it = d.cost_.begin();
+        std::advance(it, random_numbers[i]);
+        auto const end_idx = it->first;
+        auto end_lf =
+            car::label{car::node{end_idx, 0, direction::kForward}, 0U};
+        // auto end_lb = car::label{car::node{end_idx, 0, direction::kBackward},
+        // 0U};
+
+        if (it->second.cost(end_lf.get_node()) == kInfeasible) {
+          continue;
+        }
+        auto a = a_star<car>{};
+        auto h_a = cista::BASE_HASH;
+        auto n_a = 0U;
+        a.reset(opt.max_dist_);
+
+        a.add_end(w, end_lf.get_node());
+        a.add_start(w, start_lf);
+        a.add_start(w, start_lb);
+        a.run<direction::kForward, false>(w, *w.r_, opt.max_dist_, nullptr,
+                                          nullptr, elevations.get());
+
+        auto end_node = a.found_node_.value();
+        auto it_a = a.cost_.find(end_idx);
+        if (it_a == a.cost_.end()) {
+          std::cout << "Element " << end_idx << " not found in a.cost_.\n";
+          std::cout << it->second.cost(end_lf.get_node()) << std::endl;
+          throw std::runtime_error("Element not found in a.cost_");
+        } else if (d.get_cost(end_node) != a.get_cost(end_node)) {
+          std::cout << "Dijkstra end cost: " << d.get_cost(end_lf.get_node())
+                    << " A* end cost: " << a.get_cost(end_lf.get_node())
+                    << " end_idx: " << static_cast<std::uint32_t>(end_idx)
+                    << " end_lf index: "
+                    << static_cast<std::uint32_t>(end_lf.n_) << std::endl;
+          throw std::runtime_error("Costs are not equal");
+        }
+      }
+    });
+  }
+  for (auto& t : threads) {
+    t.join();
+  }
+  std::cout << "Success: Costs are equal \n";
+}

include/osr/routing/a_star.h

@@ -0,0 +1,173 @@
+#pragma once
+
+#include <unordered_set>
+#include "geo/webmercator.h"
+#include "osr/elevation_storage.h"
+#include "osr/lookup.h"
+#include "osr/routing/dial.h"
+#include "osr/types.h"
+#include "osr/ways.h"
+
+namespace osr {
+
+struct sharing_data;
+
+template <typename Profile>
+struct a_star {
+  using profile_t = Profile;
+  using key = typename Profile::key;
+  using label = typename Profile::label;
+  using node = typename Profile::node;
+  using entry = typename Profile::entry;
+  using hash = typename Profile::hash;
+
+  constexpr static auto const kDebug = false;
+
+  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(node_h{l, 0, heuristic(l, w)});
+    }
+  }
+
+  void add_end(ways const& w, node const lf) {
+    end_node_ = lf;
+    end_loc_ = w.get_node_pos(lf.n_);
+  }
+
+  void reset(cost_t const max) {
+    pq_.clear();
+    pq_.n_buckets(max + 1U);
+    cost_.clear();
+  }
+
+  struct node_h {
+    cost_t priority() const {
+      return static_cast<std::uint16_t>(cost + heuristic);
+    }
+    bool operator<(const node_h& other) const {
+      return this->priority() < other.priority();
+    }
+    bool operator>(const node_h& other) const {
+      return this->priority() > other.priority();
+    }
+    Profile::label l;
+    cost_t cost;
+    cost_t heuristic;
+  };
+
+  cost_t heuristic(label const l, ways const& w) {
+    auto const start_coord =
+        geo::latlng_to_merc(w.get_node_pos(l.n_).as_latlng());
+    auto const end_coord = geo::latlng_to_merc(end_loc_.as_latlng());
+    auto const dx = end_coord.x_ - start_coord.x_;
+    auto const dy = end_coord.y_ - start_coord.y_;
+    auto const dist = std::sqrt(dx * dx + dy * dy);
+
+    return Profile::heuristic(dist);
+  }
+
+  struct get_bucket {
+    cost_t operator()(node_h const& n) { return n.cost + n.heuristic; }
+  };
+
+  cost_t get_cost(node const n) const {
+    auto const it = cost_.find(n.get_key());
+    if (it != end(cost_))
+      return it->second.cost(n);
+    else {
+      return kInfeasible;
+    }
+  }
+
+  template <direction SearchDir, bool WithBlocked>
+  void run(ways const& w,
+           ways::routing const& r,
+           cost_t const max,
+           bitvec<node_idx_t> const* blocked,
+           sharing_data const* sharing,
+           elevation_storage const* elevations) {
+    while (!pq_.empty()) {
+      auto curr_node_h = pq_.pop();
+      auto l = curr_node_h.l;
+
+      if (l.get_node().get_node() == end_node_.value().get_node()) {
+        found_node_ = l.get_node();
+        return;
+      }
+      if (get_cost(l.get_node()) < l.cost()) {
+        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, elevations,
+          [&](node const neighbor, std::uint32_t const cost, distance_t,
+              way_idx_t const way, std::uint16_t, std::uint16_t,
+              elevation_storage::elevation const) {
+            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());
+              node_h next_h = node_h{next, next.cost_, heuristic(next, w)};
+              if (next_h.cost + next_h.heuristic < max) {
+                pq_.push(std::move(next_h));
+              }
+              if constexpr (kDebug) {
+                std::cout << " -> PUSH\n";
+              }
+            } else {
+              if constexpr (kDebug) {
+                std::cout << " -> DOMINATED\n";
+              }
+            }
+          });
+    }
+  }
+
+  void run(ways const& w,
+           ways::routing const& r,
+           cost_t const max,
+           bitvec<node_idx_t> const* blocked,
+           sharing_data const* sharing,
+           elevation_storage const* elevations,
+           direction const dir) {
+    if (blocked == nullptr) {
+      dir == direction::kForward ? run<direction::kForward, false>(
+                                       w, r, max, blocked, sharing, elevations)
+                                 : run<direction::kBackward, false>(
+                                       w, r, max, blocked, sharing, elevations);
+    } else {
+      dir == direction::kForward ? run<direction::kForward, true>(
+                                       w, r, max, blocked, sharing, elevations)
+                                 : run<direction::kBackward, true>(
+                                       w, r, max, blocked, sharing, elevations);
+    }
+  }
+
+  dial<node_h, get_bucket> pq_{get_bucket{}};
+  std::optional<node> end_node_;
+  std::optional<node> found_node_;
+  point end_loc_;
+  ankerl::unordered_dense::map<key, entry, hash> cost_;
+};
+
+}  // namespace osr

include/osr/routing/algorithms.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <cinttypes>
+#include <string_view>
+
+namespace osr {
+
+enum class routing_algorithm : std::uint8_t { kDijkstra, kAStar, kAStarBi };
+
+routing_algorithm to_algorithm(std::string_view);
+
+}  // namespace osr