[12] | 1 | |
---|
| 2 | |
---|
| 3 | // |
---|
| 4 | //======================================================================= |
---|
| 5 | // Copyright (c) 2004 Kristopher Beevers |
---|
| 6 | // |
---|
| 7 | // Distributed under the Boost Software License, Version 1.0. (See |
---|
| 8 | // accompanying file LICENSE_1_0.txt or copy at |
---|
| 9 | // http://www.boost.org/LICENSE_1_0.txt) |
---|
| 10 | //======================================================================= |
---|
| 11 | // |
---|
| 12 | |
---|
| 13 | #ifndef BOOST_GRAPH_ASTAR_SEARCH_HPP |
---|
| 14 | #define BOOST_GRAPH_ASTAR_SEARCH_HPP |
---|
| 15 | |
---|
| 16 | |
---|
| 17 | #include <functional> |
---|
| 18 | #include <boost/limits.hpp> |
---|
| 19 | #include <boost/graph/named_function_params.hpp> |
---|
| 20 | #include <boost/pending/mutable_queue.hpp> |
---|
| 21 | #include <boost/graph/relax.hpp> |
---|
| 22 | #include <boost/pending/indirect_cmp.hpp> |
---|
| 23 | #include <boost/graph/exception.hpp> |
---|
| 24 | #include <boost/graph/breadth_first_search.hpp> |
---|
| 25 | |
---|
| 26 | |
---|
| 27 | namespace boost { |
---|
| 28 | |
---|
| 29 | |
---|
| 30 | template <class Heuristic, class Graph> |
---|
| 31 | struct AStarHeuristicConcept { |
---|
| 32 | void constraints() |
---|
| 33 | { |
---|
| 34 | function_requires< CopyConstructibleConcept<Heuristic> >(); |
---|
| 35 | h(u); |
---|
| 36 | } |
---|
| 37 | Heuristic h; |
---|
| 38 | typename graph_traits<Graph>::vertex_descriptor u; |
---|
| 39 | }; |
---|
| 40 | |
---|
| 41 | |
---|
| 42 | template <class Graph, class CostType> |
---|
| 43 | class astar_heuristic : public std::unary_function< |
---|
| 44 | typename graph_traits<Graph>::vertex_descriptor, CostType> |
---|
| 45 | { |
---|
| 46 | public: |
---|
| 47 | typedef typename graph_traits<Graph>::vertex_descriptor Vertex; |
---|
| 48 | astar_heuristic() {} |
---|
| 49 | CostType operator()(Vertex u) { return static_cast<CostType>(0); } |
---|
| 50 | }; |
---|
| 51 | |
---|
| 52 | |
---|
| 53 | |
---|
| 54 | template <class Visitor, class Graph> |
---|
| 55 | struct AStarVisitorConcept { |
---|
| 56 | void constraints() |
---|
| 57 | { |
---|
| 58 | function_requires< CopyConstructibleConcept<Visitor> >(); |
---|
| 59 | vis.initialize_vertex(u, g); |
---|
| 60 | vis.discover_vertex(u, g); |
---|
| 61 | vis.examine_vertex(u, g); |
---|
| 62 | vis.examine_edge(e, g); |
---|
| 63 | vis.edge_relaxed(e, g); |
---|
| 64 | vis.edge_not_relaxed(e, g); |
---|
| 65 | vis.black_target(e, g); |
---|
| 66 | vis.finish_vertex(u, g); |
---|
| 67 | } |
---|
| 68 | Visitor vis; |
---|
| 69 | Graph g; |
---|
| 70 | typename graph_traits<Graph>::vertex_descriptor u; |
---|
| 71 | typename graph_traits<Graph>::edge_descriptor e; |
---|
| 72 | }; |
---|
| 73 | |
---|
| 74 | |
---|
| 75 | template <class Visitors = null_visitor> |
---|
| 76 | class astar_visitor : public bfs_visitor<Visitors> { |
---|
| 77 | public: |
---|
| 78 | astar_visitor() {} |
---|
| 79 | astar_visitor(Visitors vis) |
---|
| 80 | : bfs_visitor<Visitors>(vis) {} |
---|
| 81 | |
---|
| 82 | template <class Edge, class Graph> |
---|
| 83 | void edge_relaxed(Edge e, Graph& g) { |
---|
| 84 | invoke_visitors(this->m_vis, e, g, on_edge_relaxed()); |
---|
| 85 | } |
---|
| 86 | template <class Edge, class Graph> |
---|
| 87 | void edge_not_relaxed(Edge e, Graph& g) { |
---|
| 88 | invoke_visitors(this->m_vis, e, g, on_edge_not_relaxed()); |
---|
| 89 | } |
---|
| 90 | private: |
---|
| 91 | template <class Edge, class Graph> |
---|
| 92 | void tree_edge(Edge e, Graph& g) {} |
---|
| 93 | template <class Edge, class Graph> |
---|
| 94 | void non_tree_edge(Edge e, Graph& g) {} |
---|
| 95 | }; |
---|
| 96 | template <class Visitors> |
---|
| 97 | astar_visitor<Visitors> |
---|
| 98 | make_astar_visitor(Visitors vis) { |
---|
| 99 | return astar_visitor<Visitors>(vis); |
---|
| 100 | } |
---|
| 101 | typedef astar_visitor<> default_astar_visitor; |
---|
| 102 | |
---|
| 103 | |
---|
| 104 | namespace detail { |
---|
| 105 | |
---|
| 106 | template <class AStarHeuristic, class UniformCostVisitor, |
---|
| 107 | class UpdatableQueue, class PredecessorMap, |
---|
| 108 | class CostMap, class DistanceMap, class WeightMap, |
---|
| 109 | class ColorMap, class BinaryFunction, |
---|
| 110 | class BinaryPredicate> |
---|
| 111 | struct astar_bfs_visitor |
---|
| 112 | { |
---|
| 113 | |
---|
| 114 | typedef typename property_traits<CostMap>::value_type C; |
---|
| 115 | typedef typename property_traits<ColorMap>::value_type ColorValue; |
---|
| 116 | typedef color_traits<ColorValue> Color; |
---|
| 117 | |
---|
| 118 | |
---|
| 119 | astar_bfs_visitor(AStarHeuristic h, UniformCostVisitor vis, |
---|
| 120 | UpdatableQueue& Q, PredecessorMap p, |
---|
| 121 | CostMap c, DistanceMap d, WeightMap w, |
---|
| 122 | ColorMap col, BinaryFunction combine, |
---|
| 123 | BinaryPredicate compare, C zero) |
---|
| 124 | : m_h(h), m_vis(vis), m_Q(Q), m_predecessor(p), m_cost(c), |
---|
| 125 | m_distance(d), m_weight(w), m_color(col), |
---|
| 126 | m_combine(combine), m_compare(compare), m_zero(zero) {} |
---|
| 127 | |
---|
| 128 | |
---|
| 129 | template <class Vertex, class Graph> |
---|
| 130 | void initialize_vertex(Vertex u, Graph& g) { |
---|
| 131 | m_vis.initialize_vertex(u, g); |
---|
| 132 | } |
---|
| 133 | template <class Vertex, class Graph> |
---|
| 134 | void discover_vertex(Vertex u, Graph& g) { |
---|
| 135 | m_vis.discover_vertex(u, g); |
---|
| 136 | } |
---|
| 137 | template <class Vertex, class Graph> |
---|
| 138 | void examine_vertex(Vertex u, Graph& g) { |
---|
| 139 | m_vis.examine_vertex(u, g); |
---|
| 140 | } |
---|
| 141 | template <class Vertex, class Graph> |
---|
| 142 | void finish_vertex(Vertex u, Graph& g) { |
---|
| 143 | m_vis.finish_vertex(u, g); |
---|
| 144 | } |
---|
| 145 | template <class Edge, class Graph> |
---|
| 146 | void examine_edge(Edge e, Graph& g) { |
---|
| 147 | if (m_compare(get(m_weight, e), m_zero)) |
---|
| 148 | throw negative_edge(); |
---|
| 149 | m_vis.examine_edge(e, g); |
---|
| 150 | } |
---|
| 151 | template <class Edge, class Graph> |
---|
| 152 | void non_tree_edge(Edge, Graph&) {} |
---|
| 153 | |
---|
| 154 | |
---|
| 155 | |
---|
| 156 | template <class Edge, class Graph> |
---|
| 157 | void tree_edge(Edge e, Graph& g) { |
---|
| 158 | m_decreased = relax(e, g, m_weight, m_predecessor, m_distance, |
---|
| 159 | m_combine, m_compare); |
---|
| 160 | if(m_decreased) { |
---|
| 161 | m_vis.edge_relaxed(e, g); |
---|
| 162 | put(m_cost, target(e, g), |
---|
| 163 | m_combine(get(m_distance, target(e, g)), |
---|
| 164 | m_h(target(e, g)))); |
---|
| 165 | } else |
---|
| 166 | m_vis.edge_not_relaxed(e, g); |
---|
| 167 | } |
---|
| 168 | |
---|
| 169 | |
---|
| 170 | template <class Edge, class Graph> |
---|
| 171 | void gray_target(Edge e, Graph& g) { |
---|
| 172 | m_decreased = relax(e, g, m_weight, m_predecessor, m_distance, |
---|
| 173 | m_combine, m_compare); |
---|
| 174 | if(m_decreased) { |
---|
| 175 | put(m_cost, target(e, g), |
---|
| 176 | m_combine(get(m_distance, target(e, g)), |
---|
| 177 | m_h(target(e, g)))); |
---|
| 178 | m_Q.update(target(e, g)); |
---|
| 179 | m_vis.edge_relaxed(e, g); |
---|
| 180 | } else |
---|
| 181 | m_vis.edge_not_relaxed(e, g); |
---|
| 182 | } |
---|
| 183 | |
---|
| 184 | |
---|
| 185 | template <class Edge, class Graph> |
---|
| 186 | void black_target(Edge e, Graph& g) { |
---|
| 187 | m_decreased = relax(e, g, m_weight, m_predecessor, m_distance, |
---|
| 188 | m_combine, m_compare); |
---|
| 189 | if(m_decreased) { |
---|
| 190 | m_vis.edge_relaxed(e, g); |
---|
| 191 | put(m_cost, target(e, g), |
---|
| 192 | m_combine(get(m_distance, target(e, g)), |
---|
| 193 | m_h(target(e, g)))); |
---|
| 194 | m_Q.push(target(e, g)); |
---|
| 195 | put(m_color, target(e, g), Color::gray()); |
---|
| 196 | m_vis.black_target(e, g); |
---|
| 197 | } else |
---|
| 198 | m_vis.edge_not_relaxed(e, g); |
---|
| 199 | } |
---|
| 200 | |
---|
| 201 | |
---|
| 202 | |
---|
| 203 | AStarHeuristic m_h; |
---|
| 204 | UniformCostVisitor m_vis; |
---|
| 205 | UpdatableQueue& m_Q; |
---|
| 206 | PredecessorMap m_predecessor; |
---|
| 207 | CostMap m_cost; |
---|
| 208 | DistanceMap m_distance; |
---|
| 209 | WeightMap m_weight; |
---|
| 210 | ColorMap m_color; |
---|
| 211 | BinaryFunction m_combine; |
---|
| 212 | BinaryPredicate m_compare; |
---|
| 213 | bool m_decreased; |
---|
| 214 | C m_zero; |
---|
| 215 | |
---|
| 216 | }; |
---|
| 217 | |
---|
| 218 | } // namespace detail |
---|
| 219 | |
---|
| 220 | |
---|
| 221 | |
---|
| 222 | template <typename VertexListGraph, typename AStarHeuristic, |
---|
| 223 | typename AStarVisitor, typename PredecessorMap, |
---|
| 224 | typename CostMap, typename DistanceMap, |
---|
| 225 | typename WeightMap, typename ColorMap, |
---|
| 226 | typename VertexIndexMap, |
---|
| 227 | typename CompareFunction, typename CombineFunction, |
---|
| 228 | typename CostInf, typename CostZero> |
---|
| 229 | inline void |
---|
| 230 | astar_search_no_init |
---|
| 231 | (VertexListGraph &g, |
---|
| 232 | typename graph_traits<VertexListGraph>::vertex_descriptor s, |
---|
| 233 | AStarHeuristic h, AStarVisitor vis, |
---|
| 234 | PredecessorMap predecessor, CostMap cost, |
---|
| 235 | DistanceMap distance, WeightMap weight, |
---|
| 236 | ColorMap color, VertexIndexMap index_map, |
---|
| 237 | CompareFunction compare, CombineFunction combine, |
---|
| 238 | CostInf inf, CostZero zero) |
---|
| 239 | { |
---|
| 240 | typedef indirect_cmp<CostMap, CompareFunction> IndirectCmp; |
---|
| 241 | IndirectCmp icmp(cost, compare); |
---|
| 242 | |
---|
| 243 | typedef typename graph_traits<VertexListGraph>::vertex_descriptor |
---|
| 244 | Vertex; |
---|
| 245 | typedef mutable_queue<Vertex, std::vector<Vertex>, |
---|
| 246 | IndirectCmp, VertexIndexMap> |
---|
| 247 | MutableQueue; |
---|
| 248 | MutableQueue Q(num_vertices(g), icmp, index_map); |
---|
| 249 | |
---|
| 250 | detail::astar_bfs_visitor<AStarHeuristic, AStarVisitor, |
---|
| 251 | MutableQueue, PredecessorMap, CostMap, DistanceMap, |
---|
| 252 | WeightMap, ColorMap, CombineFunction, CompareFunction> |
---|
| 253 | bfs_vis(h, vis, Q, predecessor, cost, distance, weight, |
---|
| 254 | color, combine, compare, zero); |
---|
| 255 | |
---|
| 256 | breadth_first_visit(g, s, Q, bfs_vis, color); |
---|
| 257 | } |
---|
| 258 | |
---|
| 259 | |
---|
| 260 | // Non-named parameter interface |
---|
| 261 | template <typename VertexListGraph, typename AStarHeuristic, |
---|
| 262 | typename AStarVisitor, typename PredecessorMap, |
---|
| 263 | typename CostMap, typename DistanceMap, |
---|
| 264 | typename WeightMap, typename VertexIndexMap, |
---|
| 265 | typename ColorMap, |
---|
| 266 | typename CompareFunction, typename CombineFunction, |
---|
| 267 | typename CostInf, typename CostZero> |
---|
| 268 | inline void |
---|
| 269 | astar_search |
---|
| 270 | (VertexListGraph &g, |
---|
| 271 | typename graph_traits<VertexListGraph>::vertex_descriptor s, |
---|
| 272 | AStarHeuristic h, AStarVisitor vis, |
---|
| 273 | PredecessorMap predecessor, CostMap cost, |
---|
| 274 | DistanceMap distance, WeightMap weight, |
---|
| 275 | VertexIndexMap index_map, ColorMap color, |
---|
| 276 | CompareFunction compare, CombineFunction combine, |
---|
| 277 | CostInf inf, CostZero zero) |
---|
| 278 | { |
---|
| 279 | |
---|
| 280 | typedef typename property_traits<ColorMap>::value_type ColorValue; |
---|
| 281 | typedef color_traits<ColorValue> Color; |
---|
| 282 | typename graph_traits<VertexListGraph>::vertex_iterator ui, ui_end; |
---|
| 283 | for (tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) { |
---|
| 284 | put(color, *ui, Color::white()); |
---|
| 285 | put(distance, *ui, inf); |
---|
| 286 | put(cost, *ui, inf); |
---|
| 287 | put(predecessor, *ui, *ui); |
---|
| 288 | } |
---|
| 289 | put(distance, s, zero); |
---|
| 290 | put(cost, s, h(s)); |
---|
| 291 | |
---|
| 292 | astar_search_no_init |
---|
| 293 | (g, s, h, vis, predecessor, cost, distance, weight, |
---|
| 294 | color, index_map, compare, combine, inf, zero); |
---|
| 295 | |
---|
| 296 | } |
---|
| 297 | |
---|
| 298 | |
---|
| 299 | |
---|
| 300 | namespace detail { |
---|
| 301 | template <class VertexListGraph, class AStarHeuristic, |
---|
| 302 | class CostMap, class DistanceMap, class WeightMap, |
---|
| 303 | class IndexMap, class ColorMap, class Params> |
---|
| 304 | inline void |
---|
| 305 | astar_dispatch2 |
---|
| 306 | (VertexListGraph& g, |
---|
| 307 | typename graph_traits<VertexListGraph>::vertex_descriptor s, |
---|
| 308 | AStarHeuristic h, CostMap cost, DistanceMap distance, |
---|
| 309 | WeightMap weight, IndexMap index_map, ColorMap color, |
---|
| 310 | const Params& params) |
---|
| 311 | { |
---|
| 312 | dummy_property_map p_map; |
---|
| 313 | typedef typename property_traits<CostMap>::value_type C; |
---|
| 314 | astar_search |
---|
| 315 | (g, s, h, |
---|
| 316 | choose_param(get_param(params, graph_visitor), |
---|
| 317 | make_astar_visitor(null_visitor())), |
---|
| 318 | choose_param(get_param(params, vertex_predecessor), p_map), |
---|
| 319 | cost, distance, weight, index_map, color, |
---|
| 320 | choose_param(get_param(params, distance_compare_t()), |
---|
| 321 | std::less<C>()), |
---|
| 322 | choose_param(get_param(params, distance_combine_t()), |
---|
| 323 | closed_plus<C>()), |
---|
| 324 | choose_param(get_param(params, distance_inf_t()), |
---|
| 325 | std::numeric_limits<C>::max BOOST_PREVENT_MACRO_SUBSTITUTION ()), |
---|
| 326 | choose_param(get_param(params, distance_zero_t()), |
---|
| 327 | C())); |
---|
| 328 | } |
---|
| 329 | |
---|
| 330 | template <class VertexListGraph, class AStarHeuristic, |
---|
| 331 | class CostMap, class DistanceMap, class WeightMap, |
---|
| 332 | class IndexMap, class ColorMap, class Params> |
---|
| 333 | inline void |
---|
| 334 | astar_dispatch1 |
---|
| 335 | (VertexListGraph& g, |
---|
| 336 | typename graph_traits<VertexListGraph>::vertex_descriptor s, |
---|
| 337 | AStarHeuristic h, CostMap cost, DistanceMap distance, |
---|
| 338 | WeightMap weight, IndexMap index_map, ColorMap color, |
---|
| 339 | const Params& params) |
---|
| 340 | { |
---|
| 341 | typedef typename property_traits<WeightMap>::value_type D; |
---|
| 342 | typename std::vector<D>::size_type |
---|
| 343 | n = is_default_param(distance) ? num_vertices(g) : 1; |
---|
| 344 | std::vector<D> distance_map(n); |
---|
| 345 | n = is_default_param(cost) ? num_vertices(g) : 1; |
---|
| 346 | std::vector<D> cost_map(n); |
---|
| 347 | std::vector<default_color_type> color_map(num_vertices(g)); |
---|
| 348 | default_color_type c = white_color; |
---|
| 349 | |
---|
| 350 | detail::astar_dispatch2 |
---|
| 351 | (g, s, h, |
---|
| 352 | choose_param(cost, make_iterator_property_map |
---|
| 353 | (cost_map.begin(), index_map, |
---|
| 354 | cost_map[0])), |
---|
| 355 | choose_param(distance, make_iterator_property_map |
---|
| 356 | (distance_map.begin(), index_map, |
---|
| 357 | distance_map[0])), |
---|
| 358 | weight, index_map, |
---|
| 359 | choose_param(color, make_iterator_property_map |
---|
| 360 | (color_map.begin(), index_map, c)), |
---|
| 361 | params); |
---|
| 362 | } |
---|
| 363 | } // namespace detail |
---|
| 364 | |
---|
| 365 | |
---|
| 366 | // Named parameter interface |
---|
| 367 | template <typename VertexListGraph, |
---|
| 368 | typename AStarHeuristic, |
---|
| 369 | typename P, typename T, typename R> |
---|
| 370 | void |
---|
| 371 | astar_search |
---|
| 372 | (VertexListGraph &g, |
---|
| 373 | typename graph_traits<VertexListGraph>::vertex_descriptor s, |
---|
| 374 | AStarHeuristic h, const bgl_named_params<P, T, R>& params) |
---|
| 375 | { |
---|
| 376 | |
---|
| 377 | detail::astar_dispatch1 |
---|
| 378 | (g, s, h, |
---|
| 379 | get_param(params, vertex_rank), |
---|
| 380 | get_param(params, vertex_distance), |
---|
| 381 | choose_const_pmap(get_param(params, edge_weight), g, edge_weight), |
---|
| 382 | choose_const_pmap(get_param(params, vertex_index), g, vertex_index), |
---|
| 383 | get_param(params, vertex_color), |
---|
| 384 | params); |
---|
| 385 | |
---|
| 386 | } |
---|
| 387 | |
---|
| 388 | } // namespace boost |
---|
| 389 | |
---|
| 390 | #endif // BOOST_GRAPH_ASTAR_SEARCH_HPP |
---|