OR-Tools  8.1
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 
23 namespace operations_research {
24 namespace {
25 
26 // Priority queue element
27 class 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 
57 class 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 
90 void 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 
107 int 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 
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);
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 
140 void 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 
149 bool 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 
170 bool 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
integral_types.h
adjustable_priority_queue.h
AdjustablePriorityQueue::Top
T * Top()
Definition: adjustable_priority_queue.h:87
AdjustablePriorityQueue::NoteChangedPriority
void NoteChangedPriority(T *val)
Definition: adjustable_priority_queue.h:74
operations_research
The vehicle routing library lets one model and solve generic vehicle routing problems ranging from th...
Definition: dense_doubly_linked_list.h:21
AdjustablePriorityQueue::Add
void Add(T *val)
Definition: adjustable_priority_queue.h:49
int64
int64_t int64
Definition: integral_types.h:34
operations_research::AStarSP::ShortestPath
bool ShortestPath(int end_node, std::vector< int > *nodes)
Definition: astar.cc:149
operations_research::AStarSP::AStarSP
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
operations_research::AStarSP
Definition: astar.cc:57
operations_research::AStarShortestPath
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
operations_research::AStarSP::kInfinity
static const int64 kInfinity
Definition: astar.cc:59
AdjustablePriorityQueue::IsEmpty
bool IsEmpty() const
Definition: adjustable_priority_queue.h:127
AdjustablePriorityQueue< Element >
kint64max
static const int64 kint64max
Definition: integral_types.h:62
AdjustablePriorityQueue::Pop
void Pop()
Definition: adjustable_priority_queue.h:117