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