boost/graph/astar_search.hpp
// //======================================================================= // Copyright (c) 2004 Kristopher Beevers // // Distributed under the Boost Software License, Version 1.0. (See // accompanying file LICENSE_1_0.txt or copy at // http://www.boost.org/LICENSE_1_0.txt) //======================================================================= // #ifndef BOOST_GRAPH_ASTAR_SEARCH_HPP #define BOOST_GRAPH_ASTAR_SEARCH_HPP #include <functional> #include <boost/limits.hpp> #include <boost/graph/named_function_params.hpp> #include <boost/pending/mutable_queue.hpp> #include <boost/graph/relax.hpp> #include <boost/pending/indirect_cmp.hpp> #include <boost/graph/exception.hpp> #include <boost/graph/breadth_first_search.hpp> namespace boost { template <class Heuristic, class Graph> struct AStarHeuristicConcept { void constraints() { function_requires< CopyConstructibleConcept<Heuristic> >(); h(u); } Heuristic h; typename graph_traits<Graph>::vertex_descriptor u; }; template <class Graph, class CostType> class astar_heuristic : public std::unary_function< typename graph_traits<Graph>::vertex_descriptor, CostType> { public: typedef typename graph_traits<Graph>::vertex_descriptor Vertex; astar_heuristic() {} CostType operator()(Vertex u) { return static_cast<CostType>(0); } }; template <class Visitor, class Graph> struct AStarVisitorConcept { void constraints() { function_requires< CopyConstructibleConcept<Visitor> >(); vis.initialize_vertex(u, g); vis.discover_vertex(u, g); vis.examine_vertex(u, g); vis.examine_edge(e, g); vis.edge_relaxed(e, g); vis.edge_not_relaxed(e, g); vis.black_target(e, g); vis.finish_vertex(u, g); } Visitor vis; Graph g; typename graph_traits<Graph>::vertex_descriptor u; typename graph_traits<Graph>::edge_descriptor e; }; template <class Visitors = null_visitor> class astar_visitor : public bfs_visitor<Visitors> { public: astar_visitor() {} astar_visitor(Visitors vis) : bfs_visitor<Visitors>(vis) {} template <class Edge, class Graph> void edge_relaxed(Edge e, Graph& g) { invoke_visitors(this->m_vis, e, g, on_edge_relaxed()); } template <class Edge, class Graph> void edge_not_relaxed(Edge e, Graph& g) { invoke_visitors(this->m_vis, e, g, on_edge_not_relaxed()); } private: template <class Edge, class Graph> void tree_edge(Edge e, Graph& g) {} template <class Edge, class Graph> void non_tree_edge(Edge e, Graph& g) {} }; template <class Visitors> astar_visitor<Visitors> make_astar_visitor(Visitors vis) { return astar_visitor<Visitors>(vis); } typedef astar_visitor<> default_astar_visitor; namespace detail { template <class AStarHeuristic, class UniformCostVisitor, class UpdatableQueue, class PredecessorMap, class CostMap, class DistanceMap, class WeightMap, class ColorMap, class BinaryFunction, class BinaryPredicate> struct astar_bfs_visitor { typedef typename property_traits<CostMap>::value_type C; typedef typename property_traits<ColorMap>::value_type ColorValue; typedef color_traits<ColorValue> Color; typedef typename property_traits<DistanceMap>::value_type distance_type; astar_bfs_visitor(AStarHeuristic h, UniformCostVisitor vis, UpdatableQueue& Q, PredecessorMap p, CostMap c, DistanceMap d, WeightMap w, ColorMap col, BinaryFunction combine, BinaryPredicate compare, C zero) : m_h(h), m_vis(vis), m_Q(Q), m_predecessor(p), m_cost(c), m_distance(d), m_weight(w), m_color(col), m_combine(combine), m_compare(compare), m_zero(zero) {} template <class Vertex, class Graph> void initialize_vertex(Vertex u, Graph& g) { m_vis.initialize_vertex(u, g); } template <class Vertex, class Graph> void discover_vertex(Vertex u, Graph& g) { m_vis.discover_vertex(u, g); } template <class Vertex, class Graph> void examine_vertex(Vertex u, Graph& g) { m_vis.examine_vertex(u, g); } template <class Vertex, class Graph> void finish_vertex(Vertex u, Graph& g) { m_vis.finish_vertex(u, g); } template <class Edge, class Graph> void examine_edge(Edge e, Graph& g) { if (m_compare(get(m_weight, e), m_zero)) throw negative_edge(); m_vis.examine_edge(e, g); } template <class Edge, class Graph> void non_tree_edge(Edge, Graph&) {} template <class Edge, class Graph> void tree_edge(Edge e, Graph& g) { m_decreased = relax(e, g, m_weight, m_predecessor, m_distance, m_combine, m_compare); if(m_decreased) { m_vis.edge_relaxed(e, g); put(m_cost, target(e, g), m_combine(get(m_distance, target(e, g)), m_h(target(e, g)))); } else m_vis.edge_not_relaxed(e, g); } template <class Edge, class Graph> void gray_target(Edge e, Graph& g) { distance_type old_distance = get(m_distance, target(e, g)); m_decreased = relax(e, g, m_weight, m_predecessor, m_distance, m_combine, m_compare); /* On x86 Linux with optimization, we sometimes get into a horrible case where m_decreased is true but the distance hasn't actually changed. This occurs when the comparison inside relax() occurs with the 80-bit precision of the x87 floating point unit, but the difference is lost when the resulting values are written back to lower-precision memory (e.g., a double). With the eager Dijkstra's implementation, this results in looping. */ if(m_decreased && old_distance != get(m_distance, target(e, g))) { put(m_cost, target(e, g), m_combine(get(m_distance, target(e, g)), m_h(target(e, g)))); m_Q.update(target(e, g)); m_vis.edge_relaxed(e, g); } else m_vis.edge_not_relaxed(e, g); } template <class Edge, class Graph> void black_target(Edge e, Graph& g) { distance_type old_distance = get(m_distance, target(e, g)); m_decreased = relax(e, g, m_weight, m_predecessor, m_distance, m_combine, m_compare); /* See comment in gray_target */ if(m_decreased && old_distance != get(m_distance, target(e, g))) { m_vis.edge_relaxed(e, g); put(m_cost, target(e, g), m_combine(get(m_distance, target(e, g)), m_h(target(e, g)))); m_Q.push(target(e, g)); put(m_color, target(e, g), Color::gray()); m_vis.black_target(e, g); } else m_vis.edge_not_relaxed(e, g); } AStarHeuristic m_h; UniformCostVisitor m_vis; UpdatableQueue& m_Q; PredecessorMap m_predecessor; CostMap m_cost; DistanceMap m_distance; WeightMap m_weight; ColorMap m_color; BinaryFunction m_combine; BinaryPredicate m_compare; bool m_decreased; C m_zero; }; } // namespace detail template <typename VertexListGraph, typename AStarHeuristic, typename AStarVisitor, typename PredecessorMap, typename CostMap, typename DistanceMap, typename WeightMap, typename ColorMap, typename VertexIndexMap, typename CompareFunction, typename CombineFunction, typename CostInf, typename CostZero> inline void astar_search_no_init (VertexListGraph &g, typename graph_traits<VertexListGraph>::vertex_descriptor s, AStarHeuristic h, AStarVisitor vis, PredecessorMap predecessor, CostMap cost, DistanceMap distance, WeightMap weight, ColorMap color, VertexIndexMap index_map, CompareFunction compare, CombineFunction combine, CostInf inf, CostZero zero) { typedef indirect_cmp<CostMap, CompareFunction> IndirectCmp; IndirectCmp icmp(cost, compare); typedef typename graph_traits<VertexListGraph>::vertex_descriptor Vertex; typedef mutable_queue<Vertex, std::vector<Vertex>, IndirectCmp, VertexIndexMap> MutableQueue; MutableQueue Q(num_vertices(g), icmp, index_map); detail::astar_bfs_visitor<AStarHeuristic, AStarVisitor, MutableQueue, PredecessorMap, CostMap, DistanceMap, WeightMap, ColorMap, CombineFunction, CompareFunction> bfs_vis(h, vis, Q, predecessor, cost, distance, weight, color, combine, compare, zero); breadth_first_visit(g, s, Q, bfs_vis, color); } // Non-named parameter interface template <typename VertexListGraph, typename AStarHeuristic, typename AStarVisitor, typename PredecessorMap, typename CostMap, typename DistanceMap, typename WeightMap, typename VertexIndexMap, typename ColorMap, typename CompareFunction, typename CombineFunction, typename CostInf, typename CostZero> inline void astar_search (VertexListGraph &g, typename graph_traits<VertexListGraph>::vertex_descriptor s, AStarHeuristic h, AStarVisitor vis, PredecessorMap predecessor, CostMap cost, DistanceMap distance, WeightMap weight, VertexIndexMap index_map, ColorMap color, CompareFunction compare, CombineFunction combine, CostInf inf, CostZero zero) { typedef typename property_traits<ColorMap>::value_type ColorValue; typedef color_traits<ColorValue> Color; typename graph_traits<VertexListGraph>::vertex_iterator ui, ui_end; for (tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) { put(color, *ui, Color::white()); put(distance, *ui, inf); put(cost, *ui, inf); put(predecessor, *ui, *ui); vis.initialize_vertex(*ui, g); } put(distance, s, zero); put(cost, s, h(s)); astar_search_no_init (g, s, h, vis, predecessor, cost, distance, weight, color, index_map, compare, combine, inf, zero); } namespace detail { template <class VertexListGraph, class AStarHeuristic, class CostMap, class DistanceMap, class WeightMap, class IndexMap, class ColorMap, class Params> inline void astar_dispatch2 (VertexListGraph& g, typename graph_traits<VertexListGraph>::vertex_descriptor s, AStarHeuristic h, CostMap cost, DistanceMap distance, WeightMap weight, IndexMap index_map, ColorMap color, const Params& params) { dummy_property_map p_map; typedef typename property_traits<CostMap>::value_type C; astar_search (g, s, h, choose_param(get_param(params, graph_visitor), make_astar_visitor(null_visitor())), choose_param(get_param(params, vertex_predecessor), p_map), cost, distance, weight, index_map, color, choose_param(get_param(params, distance_compare_t()), std::less<C>()), choose_param(get_param(params, distance_combine_t()), closed_plus<C>()), choose_param(get_param(params, distance_inf_t()), std::numeric_limits<C>::max BOOST_PREVENT_MACRO_SUBSTITUTION ()), choose_param(get_param(params, distance_zero_t()), C())); } template <class VertexListGraph, class AStarHeuristic, class CostMap, class DistanceMap, class WeightMap, class IndexMap, class ColorMap, class Params> inline void astar_dispatch1 (VertexListGraph& g, typename graph_traits<VertexListGraph>::vertex_descriptor s, AStarHeuristic h, CostMap cost, DistanceMap distance, WeightMap weight, IndexMap index_map, ColorMap color, const Params& params) { typedef typename property_traits<WeightMap>::value_type D; typename std::vector<D>::size_type n = is_default_param(distance) ? num_vertices(g) : 1; std::vector<D> distance_map(n); n = is_default_param(cost) ? num_vertices(g) : 1; std::vector<D> cost_map(n); std::vector<default_color_type> color_map(num_vertices(g)); default_color_type c = white_color; detail::astar_dispatch2 (g, s, h, choose_param(cost, make_iterator_property_map (cost_map.begin(), index_map, cost_map[0])), choose_param(distance, make_iterator_property_map (distance_map.begin(), index_map, distance_map[0])), weight, index_map, choose_param(color, make_iterator_property_map (color_map.begin(), index_map, c)), params); } } // namespace detail // Named parameter interface template <typename VertexListGraph, typename AStarHeuristic, typename P, typename T, typename R> void astar_search (VertexListGraph &g, typename graph_traits<VertexListGraph>::vertex_descriptor s, AStarHeuristic h, const bgl_named_params<P, T, R>& params) { detail::astar_dispatch1 (g, s, h, get_param(params, vertex_rank), get_param(params, vertex_distance), choose_const_pmap(get_param(params, edge_weight), g, edge_weight), choose_const_pmap(get_param(params, vertex_index), g, vertex_index), get_param(params, vertex_color), params); } } // namespace boost #endif // BOOST_GRAPH_ASTAR_SEARCH_HPP