14 #include <absl/container/flat_hash_map.h>
15 #include <absl/container/flat_hash_set.h>
30 : heap_index_(-1), distance_(0), node_(-1), distance_with_heuristic_(0) {}
34 bool operator<(
const Element& other)
const {
35 return distance_with_heuristic_ > other.distance_with_heuristic_;
37 void SetHeapIndex(
int h) { heap_index_ = h; }
38 int GetHeapIndex()
const {
return heap_index_; }
39 void set_distance(
int64 distance) { distance_ = distance; }
40 void set_distance_with_heuristic(
int64 distance_with_heuristic) {
41 distance_with_heuristic_ = distance_with_heuristic;
43 int64 distance_with_heuristic() {
return distance_with_heuristic_; }
45 int64 distance()
const {
return distance_; }
46 void set_node(
int node) { node_ = node; }
47 int node()
const {
return node_; }
52 int64 distance_with_heuristic_;
61 AStarSP(
int node_count,
int start_node, std::function<
int64(
int,
int)> graph,
62 std::function<
int64(
int)> heuristic,
int64 disconnected_distance)
63 : node_count_(node_count),
64 start_node_(start_node),
65 graph_(std::move(graph)),
66 disconnected_distance_(disconnected_distance),
67 predecessor_(new int[node_count]),
68 elements_(node_count),
69 heuristic_(std::move(heuristic)) {}
70 bool ShortestPath(
int end_node, std::vector<int>* nodes);
74 int SelectClosestNode(
int64* distance);
75 void Update(
int label);
76 void FindPath(
int dest, std::vector<int>* nodes);
78 const int node_count_;
79 const int start_node_;
80 std::function<
int64(
int,
int)> graph_;
81 std::function<
int64(
int)> heuristic_;
82 const int64 disconnected_distance_;
83 std::unique_ptr<int[]> predecessor_;
85 std::vector<Element> elements_;
86 absl::flat_hash_set<int> not_visited_;
87 absl::flat_hash_set<int> added_to_the_frontier_;
90 void AStarSP::Initialize() {
91 for (
int i = 0; i < node_count_; i++) {
92 elements_[i].set_node(i);
93 if (i == start_node_) {
95 elements_[i].set_distance(0);
96 elements_[i].set_distance_with_heuristic(heuristic_(i));
97 frontier_.
Add(&elements_[i]);
100 elements_[i].set_distance_with_heuristic(
kInfinity);
101 predecessor_[i] = start_node_;
102 not_visited_.insert(i);
107 int AStarSP::SelectClosestNode(
int64* distance) {
108 const int node = frontier_.
Top()->node();
109 *distance = frontier_.
Top()->distance();
111 not_visited_.erase(node);
112 added_to_the_frontier_.erase(node);
116 void AStarSP::Update(
int node) {
117 for (absl::flat_hash_set<int>::const_iterator it = not_visited_.begin();
118 it != not_visited_.end(); ++it) {
119 const int other_node = *it;
120 const int64 graph_node_i = graph_(node, other_node);
121 if (graph_node_i != disconnected_distance_) {
122 if (added_to_the_frontier_.find(other_node) ==
123 added_to_the_frontier_.end()) {
124 frontier_.
Add(&elements_[other_node]);
125 added_to_the_frontier_.insert(other_node);
128 const int64 other_distance = elements_[node].distance() + graph_node_i;
129 if (elements_[other_node].distance() > other_distance) {
130 elements_[other_node].set_distance(other_distance);
131 elements_[other_node].set_distance_with_heuristic(
132 other_distance + heuristic_(other_node));
134 predecessor_[other_node] = node;
140 void AStarSP::FindPath(
int dest, std::vector<int>* nodes) {
143 while (predecessor_[j] != -1) {
144 nodes->push_back(predecessor_[j]);
154 int node = SelectClosestNode(&distance);
158 }
else if (node == end_node) {
165 FindPath(end_node, nodes);
171 std::function<
int64(
int,
int)> graph,
172 std::function<
int64(
int)> heuristic,
173 int64 disconnected_distance, std::vector<int>* nodes) {
174 AStarSP bf(node_count, start_node, std::move(graph), std::move(heuristic),
175 disconnected_distance);