OR-Tools  8.2
astar.cc
Go to the documentation of this file.
1// Copyright 2010-2018 Google LLC
2// Licensed under the Apache License, Version 2.0 (the "License");
3// you may not use this file except in compliance with the License.
4// You may obtain a copy of the License at
5//
6// http://www.apache.org/licenses/LICENSE-2.0
7//
8// Unless required by applicable law or agreed to in writing, software
9// distributed under the License is distributed on an "AS IS" BASIS,
10// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
11// See the License for the specific language governing permissions and
12// limitations under the License.
13
14#include <absl/container/flat_hash_map.h>
15#include <absl/container/flat_hash_set.h>
16
17#include <memory>
18#include <vector>
19
22
23namespace operations_research {
24namespace {
25
26// Priority queue element
27class Element {
28 public:
29 Element()
30 : heap_index_(-1), distance_(0), node_(-1), distance_with_heuristic_(0) {}
31
32 // The distance_with_heuristic is used for the comparison
33 // in the priority queue
34 bool operator<(const Element& other) const {
35 return distance_with_heuristic_ > other.distance_with_heuristic_;
36 }
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;
42 }
43 int64 distance_with_heuristic() { return distance_with_heuristic_; }
44
45 int64 distance() const { return distance_; }
46 void set_node(int node) { node_ = node; }
47 int node() const { return node_; }
48
49 private:
50 int heap_index_;
51 int64 distance_;
52 int64 distance_with_heuristic_;
53 int node_;
54};
55} // namespace
56
57class AStarSP {
58 public:
59 static const int64 kInfinity = kint64max / 2;
60
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);
71
72 private:
73 void Initialize();
74 int SelectClosestNode(int64* distance);
75 void Update(int label);
76 void FindPath(int dest, std::vector<int>* nodes);
77
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_;
88};
89
90void AStarSP::Initialize() {
91 for (int i = 0; i < node_count_; i++) {
92 elements_[i].set_node(i);
93 if (i == start_node_) {
94 predecessor_[i] = -1;
95 elements_[i].set_distance(0);
96 elements_[i].set_distance_with_heuristic(heuristic_(i));
97 frontier_.Add(&elements_[i]);
98 } else {
99 elements_[i].set_distance(kInfinity);
100 elements_[i].set_distance_with_heuristic(kInfinity);
101 predecessor_[i] = start_node_;
102 not_visited_.insert(i);
103 }
104 }
105}
106
107int AStarSP::SelectClosestNode(int64* distance) {
108 const int node = frontier_.Top()->node();
109 *distance = frontier_.Top()->distance();
110 frontier_.Pop();
111 not_visited_.erase(node);
112 added_to_the_frontier_.erase(node);
113 return node;
114}
115
116void 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);
126 }
127
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));
133 frontier_.NoteChangedPriority(&elements_[other_node]);
134 predecessor_[other_node] = node;
135 }
136 }
137 }
138}
139
140void AStarSP::FindPath(int dest, std::vector<int>* nodes) {
141 int j = dest;
142 nodes->push_back(j);
143 while (predecessor_[j] != -1) {
144 nodes->push_back(predecessor_[j]);
145 j = predecessor_[j];
146 }
147}
148
149bool AStarSP::ShortestPath(int end_node, std::vector<int>* nodes) {
150 Initialize();
151 bool found = false;
152 while (!frontier_.IsEmpty()) {
153 int64 distance;
154 int node = SelectClosestNode(&distance);
155 if (distance == kInfinity) {
156 found = false;
157 break;
158 } else if (node == end_node) {
159 found = true;
160 break;
161 }
162 Update(node);
163 }
164 if (found) {
165 FindPath(end_node, nodes);
166 }
167 return found;
168}
169
170bool AStarShortestPath(int node_count, int start_node, int end_node,
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);
176 return bf.ShortestPath(end_node, nodes);
177}
178} // namespace operations_research
static const int64 kInfinity
Definition: astar.cc:59
bool ShortestPath(int end_node, std::vector< int > *nodes)
Definition: astar.cc:149
AStarSP(int node_count, int start_node, std::function< int64(int, int)> graph, std::function< int64(int)> heuristic, int64 disconnected_distance)
Definition: astar.cc:61
static const int64 kint64max
int64_t int64
The vehicle routing library lets one model and solve generic vehicle routing problems ranging from th...
bool AStarShortestPath(int node_count, int start_node, int end_node, std::function< int64(int, int)> graph, std::function< int64(int)> heuristic, int64 disconnected_distance, std::vector< int > *nodes)
Definition: astar.cc:170
STL namespace.