root/trunk/core/src/shooting_star_search.hpp

Revision 140, 19.7 KB (checked in by anton, 3 years ago)

found_goal exception fixed
Line 
1//
2//=======================================================================
3// Copyright (c) 2004 Kristopher Beevers
4//               2007 Anton A. Patrushev, Orkney Inc.
5//
6// Distributed under the Boost Software License, Version 1.0. (See
7// accompanying file LICENSE_1_0.txt or copy at
8// http://www.boost.org/LICENSE_1_0.txt)
9//=======================================================================
10//
11
12#ifndef BOOST_GRAPH_SHOOTING_STAR_SEARCH_HPP
13#define BOOST_GRAPH_SHOOTING_STAR_SEARCH_HPP
14
15#define MAX_RULE_LENGTH 5
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/pending/relaxed_heap.hpp>
22#include <shooting_star_relax.hpp>
23#include <boost/pending/indirect_cmp.hpp>
24#include <boost/graph/exception.hpp>
25#include <boost/graph/breadth_first_search.hpp>
26
27#include <queue>
28#include "edge_visitors.hpp"
29
30template <class Edge>
31struct EdgeRankCompare
32{
33  bool operator()(const Edge& LHS, const Edge& RHS)
34  {
35    return (LHS.rank < RHS.rank);
36  }
37               
38};
39
40template <class Edge, class Container, class Cmp>
41class edge_queue : public std::priority_queue<Edge, Container, Cmp>
42{
43protected:
44  using std::priority_queue< Edge, Container, Cmp >::c;
45  using std::priority_queue< Edge, Container, Cmp >::comp;
46   
47public:
48  void sort()
49  {
50    sort_heap(c.begin(), c.end(), comp);
51  }   
52};
53
54namespace boost
55{
56 
57  template <class Heuristic, class Graph>
58  struct AStarHeuristicConcept
59  {
60    void constraints()
61    {
62      function_requires< CopyConstructibleConcept<Heuristic> >();
63      h(u);
64    }
65    Heuristic h;
66    typename graph_traits<Graph>::vertex_descriptor u;
67  };
68 
69   
70
71  template <class Graph, class CostType>
72  class astar_heuristic : public std::unary_function<
73    typename graph_traits<Graph>::vertex_descriptor, CostType>
74  {
75  public:
76    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
77    astar_heuristic() {}
78    CostType operator()(Vertex u) { return static_cast<CostType>(0); }
79  };
80 
81 
82  template <class Visitor, class Graph>
83  struct ShootingStarVisitorConcept {
84    void constraints()
85    {
86      function_requires< CopyConstructibleConcept<Visitor> >();
87      vis.initialize_vertex(u, g);
88      vis.discover_vertex(u, g);
89      vis.examine_vertex(u, g);
90      vis.examine_edge(e, g, e_max_id);
91      vis.black_target(e, pe, g, e_max_id);
92      vis.gray_target(e, pe, g, e_max_id);
93      vis.finish_vertex(u, g);
94     
95      vis.tree_edge(e, pe, g, e_max_id);
96
97      vis.initialize_edge(e, g);
98      vis.discover_edge(e, g);
99      vis.finish_edge(e, g);
100
101    }
102    Visitor vis;
103    Graph g;
104    typename graph_traits<Graph>::vertex_descriptor u;
105    typename graph_traits<Graph>::edge_descriptor e, pe;
106    int e_max_id;
107  };
108 
109 
110  // Main visitor function.
111  // It decides what to do with an edge of certain color
112 
113  template <class IncidenceGraph, class Buffer, class BFSVisitor,
114            class ColorMap, class EdgeColorMap/*, class HistoryMap*/>
115  void shooting_star_edges_visit (const IncidenceGraph& g,
116                            typename graph_traits<IncidenceGraph>::edge_descriptor s,
117                            Buffer& Q, BFSVisitor vis, ColorMap color, EdgeColorMap edge_color,
118                            int e_max_id)
119  {
120    function_requires< IncidenceGraphConcept<IncidenceGraph> >();
121    typedef graph_traits<IncidenceGraph> GTraits;
122    typedef typename GTraits::vertex_descriptor Vertex;
123    typedef typename GTraits::edge_descriptor Edge;
124    function_requires< ShootingStarVisitorConcept<BFSVisitor, IncidenceGraph> >();
125    function_requires< ReadWritePropertyMapConcept<EdgeColorMap, Edge> >();
126   
127    typedef typename property_traits<ColorMap>::value_type ColorValue;
128    typedef color_traits<ColorValue> Color;
129
130    typedef typename property_traits<EdgeColorMap>::value_type EdgeColorValue;
131    typedef color_traits<EdgeColorValue> EdgeColor;
132
133    typename GTraits::out_edge_iterator ei, ei_end;
134
135    // Paint source edge gray
136    put(edge_color, s, EdgeColor::gray());
137    vis.discover_edge(s, g);
138   
139    // Enqueue the source edge
140    Q.push(s);
141   
142    // While the queue is not empty
143    while (! Q.empty())
144    {
145      // Get an edge from the top
146      Edge e = Q.top();  Q.pop();           
147     
148      // Examine the edge
149      vis.examine_edge(e, g, e_max_id);
150     
151      // For all adjacent edges for the current one
152      for (tie(ei, ei_end) = out_edges(target(e, g), g); ei != ei_end; ++ei)
153      {
154        // Get a color
155        EdgeColorValue e_color = get(edge_color, *ei);
156
157        // Discover the edge and paint it grey
158        // and enqueue it if it was white
159        if (e_color == EdgeColor::white())
160        {         
161          vis.tree_edge(*ei, e, g, e_max_id);
162          put(edge_color, *ei, EdgeColor::gray());   
163          vis.discover_edge(*ei, g);
164          Q.push(*ei);
165        }
166        // or execute appropriate function
167        // if it was grey or black
168        else
169        {                                     
170          vis.non_tree_edge(*ei, g);
171          if (e_color == EdgeColor::gray())
172          {         
173            vis.gray_target(*ei, e, g, e_max_id);
174          }
175          else
176          {                                     
177            vis.black_target(*ei, e, g, e_max_id);
178          }
179        }
180       
181      } // end for
182
183      // Finally paint the parent edge black
184      put(edge_color, e, EdgeColor::black());       
185      vis.finish_edge(e, g);
186     
187    } // end while
188  }
189                                                                                                                                                                                             
190 
191  template <class Visitors = null_visitor>
192  class shooting_star_visitor : public bfs_visitor<Visitors>
193  {
194    public:
195      shooting_star_visitor() {}
196      shooting_star_visitor(Visitors vis)
197        : bfs_visitor<Visitors>(vis) {}
198 
199      template <class Edge, class Graph>
200      void edge_relaxed(Edge e, Graph& g)
201      {
202        invoke_visitors(this->m_vis, e, g, on_edge_relaxed());
203      }
204      template <class Edge, class Graph>
205      void edge_not_relaxed(Edge e, Graph& g)
206      {
207        invoke_visitors(this->m_vis, e, g, on_edge_not_relaxed());     
208      }
209      template <class Edge, class Graph>
210      void initialize_edge(Edge e, Graph& g)
211      {
212        invoke_visitors(this->m_vis, e, g, on_initialize_edge());     
213      }
214   
215    private:
216      template <class Edge, class Graph>
217      void tree_edge(Edge e, Edge pe, Graph& g) {}
218      template <class Edge, class Graph>
219      void non_tree_edge(Edge e, Graph& g) {}
220  };
221
222  template <class Visitors>
223  shooting_star_visitor<Visitors>
224  make_shooting_star_visitor(Visitors vis)
225  {
226    return shooting_star_visitor<Visitors>(vis);
227  }
228 
229  typedef shooting_star_visitor<> default_shooting_star_visitor;
230 
231
232  namespace detail {
233
234    // Shooting* visitor
235    // based on BFS visitor concept from BGL   
236    template <class AStarHeuristic, class UniformCostVisitor,
237              class UpdatableQueue, class PredecessorMap,
238              class CostMap,
239              class DistanceMap, class WeightMap,
240              class EdgeMap,
241              class ColorMap, class EdgeColorMap,
242              class BinaryFunction,
243              class BinaryPredicate>
244    struct shooting_star_bfs_visitor
245    {
246     
247      typedef typename property_traits<CostMap>::value_type C;
248      typedef typename property_traits<ColorMap>::value_type ColorValue;
249      typedef color_traits<ColorValue> Color;
250
251      typedef typename property_traits<EdgeColorMap>::value_type EdgeColorValue;
252      typedef color_traits<ColorValue> EdgeColor;
253
254      typedef typename property_traits<DistanceMap>::value_type distance_type;
255     
256      shooting_star_bfs_visitor(AStarHeuristic h, UniformCostVisitor vis,
257                        UpdatableQueue& Q, PredecessorMap& p,
258                        CostMap c, DistanceMap d, WeightMap w, EdgeMap em,
259                        ColorMap col, EdgeColorMap ecol, //HistoryMap his,
260                        BinaryFunction combine,
261                        BinaryPredicate compare, C zero)
262        : m_h(h), m_vis(vis), m_Q(Q), m_predecessor(p), m_cost(c),
263          m_distance(d), m_weight(w), m_edge(em), m_color(col),
264          m_ecolor(ecol), //m_history(his),
265          m_combine(combine), m_compare(compare), m_zero(zero) {}
266
267      shooting_star_bfs_visitor(const shooting_star_bfs_visitor &v)
268        : m_h(v.m_h), m_vis(v.m_vis), m_Q(v.m_Q), m_predecessor(v.m_predecessor), m_cost(v.m_cost),
269          m_distance(v.m_distance), m_weight(v.m_weight), m_edge(v.m_edge), m_color(v.m_color),
270          m_ecolor(v.m_ecolor), //m_history(his),
271          m_combine(v.m_combine), m_compare(v.m_compare), m_zero(v.m_zero) {}
272         
273      ~shooting_star_bfs_visitor() {}
274     
275      template <class Vertex, class Graph>
276      void initialize_vertex(Vertex u, Graph& g)
277      {
278        m_vis.initialize_vertex(u, g);
279      }
280      template <class Edge, class Graph>
281      void initialize_edge(Edge e, Graph& g)
282      {
283        m_vis.initialize_edge(e, g);
284      }
285      template <class Vertex, class Graph>
286      void discover_vertex(Vertex u, Graph& g)
287      {
288        m_vis.discover_vertex(u, g);
289      }
290      template <class Edge, class Graph>
291      void discover_edge(Edge e, Graph& g)
292      {
293        m_vis.discover_vertex(e, g);
294      }
295      template <class Vertex, class Graph>
296      void examine_vertex(Vertex u, Graph& g)
297      {
298        m_vis.examine_vertex(u, g);
299      }
300      template <class Vertex, class Graph>
301      void finish_vertex(Vertex u, Graph& g)
302      {
303        m_vis.finish_vertex(u, g);
304      }
305      template <class Edge, class Graph>
306      void examine_edge(Edge e, Graph& g, int e_max_id)
307      {
308        if (m_compare(get(m_weight, e), m_zero))
309          throw negative_edge();
310        m_vis.examine_edge(e, g, e_max_id);
311      }
312      template <class Edge, class Graph>
313      void non_tree_edge(Edge, Graph&) {}
314     
315      template <class Edge, class Graph>
316      void finish_edge(Edge e, Graph& g)
317      {
318        m_vis.finish_edge(e, g);
319      }
320     
321     
322      template <class Edge, class Graph>
323      void tree_edge(Edge e, Edge pe, Graph& g, int e_max_id)
324      {
325        m_decreased = relax(e, pe, g, m_weight, m_edge, m_predecessor, m_distance,
326                            m_cost, m_combine, m_compare, e_max_id);
327   
328        if(m_decreased)
329        {
330          m_vis.edge_relaxed(e, g);
331         
332          put(m_cost, e,
333              m_combine(get(m_distance, e),
334                        m_h(e)));
335
336        }
337        else
338          m_vis.edge_not_relaxed(e, g);
339      }
340     
341     
342      template <class Edge, class Graph>
343      void gray_target(Edge e, Edge pe, Graph& g, int e_max_id)
344      {
345        m_decreased = relax(e, pe, g, m_weight, m_edge, m_predecessor, m_distance,
346                            m_cost, m_combine, m_compare, e_max_id);
347
348        if(m_decreased)
349        {
350          put(m_cost, e,
351              m_combine(get(m_distance, e),
352                        m_h(e)));
353                       
354          m_Q.update(e);
355                 
356          m_vis.edge_relaxed(e, g);
357        }
358        else
359          m_vis.edge_not_relaxed(e, g);
360      }
361     
362           
363      template <class Edge, class Graph>
364      void black_target(Edge e, Edge pe, Graph& g, int e_max_id)
365      {
366
367        m_decreased = relax(e, pe, g, m_weight, m_edge, m_predecessor, m_distance,
368                            m_cost, m_combine, m_compare, e_max_id);
369
370        if(m_decreased)
371        {
372          m_vis.edge_relaxed(e, g);
373          put(m_cost, e, m_combine(get(m_distance, e), m_h(e)));
374         
375          m_Q.push(e);
376          put(m_ecolor, e, EdgeColor::gray());
377          m_vis.black_target(e, g);
378        }
379        else
380          m_vis.edge_not_relaxed(e, g);
381      } 
382
383      AStarHeuristic m_h;
384      UniformCostVisitor m_vis;
385      UpdatableQueue& m_Q;
386      PredecessorMap& m_predecessor;
387      CostMap m_cost;
388      EdgeMap m_edge;
389      DistanceMap m_distance;
390      WeightMap m_weight;
391      ColorMap m_color;
392      EdgeColorMap m_ecolor;
393      BinaryFunction m_combine;
394      BinaryPredicate m_compare;
395      bool m_decreased;
396      C m_zero;     
397    };
398   
399  } // namespace detail
400
401  template <typename VertexAndEdgeListGraph, typename AStarHeuristic,
402            typename ShootingStarVisitor, typename PredecessorMap,
403            typename CostMap,
404            typename DistanceMap,
405            typename WeightMap,
406            typename EdgeMap,
407            typename ColorMap, typename EdgeColorMap,
408            typename IndexMap,
409            typename CompareFunction, typename CombineFunction,
410            typename CostInf, typename CostZero>
411  inline void
412  shooting_star_search_no_init
413    (VertexAndEdgeListGraph &g,
414     typename graph_traits<VertexAndEdgeListGraph>::edge_descriptor s,
415     AStarHeuristic h, ShootingStarVisitor vis,
416     PredecessorMap &predecessor, CostMap cost,
417     DistanceMap distance, WeightMap weight, EdgeMap edge_map,
418     ColorMap color, EdgeColorMap edge_color,
419     IndexMap index_map, CompareFunction compare, CombineFunction combine,
420     CostInf inf, CostZero zero, int e_max_id)
421  {
422    typedef indirect_cmp<CostMap, CompareFunction> IndirectCmp;
423    IndirectCmp icmp(cost, compare);
424 
425    typedef typename graph_traits<VertexAndEdgeListGraph>::edge_descriptor
426      Edge;
427   
428    // Queue to store the list of edges to examine.
429    // I really hate this queue for what it does with the memory sometimes.
430    //
431    //And by the way...
432    //
433    //This place is for rent :)
434    //
435    typedef mutable_queue<Edge, std::vector<Edge>, IndirectCmp, IndexMap>
436      MutableQueue;
437    MutableQueue Q(num_edges(g), icmp, index_map);
438
439    detail::shooting_star_bfs_visitor<AStarHeuristic, ShootingStarVisitor,
440        MutableQueue, PredecessorMap, CostMap, DistanceMap,
441        WeightMap, EdgeMap, ColorMap, EdgeColorMap, /*HistoryMap,*/ CombineFunction, CompareFunction>
442      bfs_vis(h, vis, Q, predecessor, cost, distance, weight,
443              edge_map, color, edge_color, combine, compare, zero);
444 
445    shooting_star_edges_visit(g, s, Q, bfs_vis, color, edge_color, e_max_id);
446  }
447 
448  // Non-named parameter interface
449  template <typename VertexAndEdgeListGraph, typename AStarHeuristic,
450            typename ShootingStarVisitor, typename PredecessorMap,
451            typename CostMap, typename DistanceMap,
452            typename WeightMap, typename EdgeMap,
453            typename IndexMap,
454            typename ColorMap, typename EdgeColorMap,
455            //typename HistoryMap,
456            typename CompareFunction, typename CombineFunction,
457            typename CostInf, typename CostZero>
458  inline void
459  shooting_star_search
460    (VertexAndEdgeListGraph &g,
461     typename graph_traits<VertexAndEdgeListGraph>::edge_descriptor s,
462     AStarHeuristic h, ShootingStarVisitor vis,
463     PredecessorMap &predecessor, CostMap cost,
464     DistanceMap distance, WeightMap weight,
465     EdgeMap edge_map, IndexMap index_map, ColorMap color, EdgeColorMap edge_color,
466     CompareFunction compare, CombineFunction combine,
467     CostInf inf, CostZero zero, int e_max_id)
468  {
469   
470    typedef typename property_traits<ColorMap>::value_type ColorValue;
471    typedef color_traits<ColorValue> Color;
472   
473    typedef typename property_traits<EdgeColorMap>::value_type EdgeColorValue;
474    typedef color_traits<EdgeColorValue> EdgeColor;
475
476    typename graph_traits<VertexAndEdgeListGraph>::vertex_iterator ui, ui_end;
477    typename graph_traits<VertexAndEdgeListGraph>::edge_iterator ei, ei_end;
478
479    for (tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui)
480    {
481      vis.initialize_vertex(*ui, g);
482    }
483
484    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
485    {
486      put(distance, *ei, inf);
487      put(edge_color, *ei, EdgeColor::white());
488      predecessor[g[*ei].id] = *ei;
489      put(cost, *ei, inf);
490    }
491
492    put(distance, s, zero);
493
494    put(cost, s, h(s));
495
496    shooting_star_search_no_init
497      (g, s, h, vis, predecessor, cost, distance, weight, edge_map,
498       color, edge_color, index_map, compare, combine, inf, zero, e_max_id);
499   
500  }
501 
502  namespace detail
503  {
504    template <class VertexAndEdgeListGraph, class AStarHeuristic,
505              class CostMap, class DistanceMap, class WeightMap, class EdgeMap,
506              class IndexMap,
507              class PredecessorMap,
508              class ColorMap, class EdgeColorMap,
509              class Params>
510    inline void
511    shooting_star_dispatch2
512      (VertexAndEdgeListGraph& g,
513       typename graph_traits<VertexAndEdgeListGraph>::edge_descriptor s,
514       AStarHeuristic h, CostMap cost, DistanceMap distance,
515       WeightMap weight, EdgeMap edge_map, IndexMap index_map,
516       PredecessorMap& predecessor, ColorMap color,
517       EdgeColorMap edge_color, const Params& params,
518       int e_max_id)
519    {
520      dummy_property_map p_map;
521      typedef typename property_traits<CostMap>::value_type C;
522      shooting_star_search
523        (g, s, h,
524         choose_param(get_param(params, graph_visitor),
525                      make_shooting_star_visitor(null_visitor())),
526         predecessor,
527         cost, distance, weight, edge_map, index_map, color, edge_color, //history,
528         choose_param(get_param(params, distance_compare_t()),
529                      std::less<C>()),
530         choose_param(get_param(params, distance_combine_t()),
531                      closed_plus<C>()),
532         choose_param(get_param(params, distance_inf_t()),
533                      std::numeric_limits<C>::max BOOST_PREVENT_MACRO_SUBSTITUTION ()),
534         choose_param(get_param(params, distance_zero_t()),
535                      C()),
536         e_max_id
537        );
538    }
539 
540    template <class VertexAndEdgeListGraph, class AStarHeuristic,
541              class CostMap, class DistanceMap, class WeightMap,
542              class EdgeMap, class IndexMap,
543              class PredecessorMap,
544              class ColorMap, class EdgeColorMap,
545              class Params>
546    inline void
547    shooting_star_dispatch1
548      (VertexAndEdgeListGraph& g,
549       typename graph_traits<VertexAndEdgeListGraph>::edge_descriptor s,
550       AStarHeuristic h, CostMap cost, DistanceMap distance,
551       WeightMap weight, EdgeMap edge_map, IndexMap index_map, PredecessorMap& predecessor,
552       ColorMap color, EdgeColorMap edge_color, const Params& params,
553       int e_max_id)
554    {
555      typedef typename property_traits<WeightMap>::value_type D;
556      typename std::vector<D>::size_type
557        n = is_default_param(distance) ? num_vertices(g) : 1;
558      std::vector<D> distance_map(n);
559      n = is_default_param(cost) ? num_vertices(g) : 1;
560      std::vector<D> cost_map(n);
561
562      std::vector<default_color_type> color_map(num_vertices(g));
563      default_color_type c = white_color;
564     
565      detail::shooting_star_dispatch2
566        (g, s, h,
567         choose_param(cost, make_iterator_property_map
568                      (cost_map.begin(), index_map,
569                       cost_map[0])),
570         choose_param(distance, make_iterator_property_map
571                      (distance_map.begin(), index_map,
572                       distance_map[0])),
573         weight, edge_map, index_map,
574         predecessor,
575         choose_param(color, make_iterator_property_map
576                      (color_map.begin(), index_map, c)),
577         edge_color,
578         params,
579         e_max_id);
580    }
581  } // namespace detail
582 
583  // Named parameter interface
584  template <typename VertexAndEdgeListGraph,
585            typename AStarHeuristic,
586            typename P, typename T, typename R,
587            typename IndexMap,
588            typename DistanceMap,
589            typename CostMap,
590            typename PredecessorMap>
591  void
592  shooting_star_search
593    (VertexAndEdgeListGraph &g,
594     typename graph_traits<VertexAndEdgeListGraph>::edge_descriptor s,
595     AStarHeuristic h, const bgl_named_params<P, T, R>& params,
596     IndexMap edge_index,
597     DistanceMap distance,
598     CostMap cost,
599     PredecessorMap &pre_map,
600     int e_max_id)
601  {
602    typedef typename graph_traits<VertexAndEdgeListGraph>::edge_descriptor tEdge;
603
604    detail::shooting_star_dispatch1
605      (g, s, h,
606       cost,
607       distance,
608       choose_const_pmap(get_param(params, edge_weight), g, edge_weight), //weight
609       choose_const_pmap(get_param(params, edge_weight2), g, edge_weight2), //adjacent edges cost
610       edge_index,
611       pre_map, //predecessors
612       get_param(params, vertex_color), //vertex color (deprecated)
613       get_param(params, edge_color), //edge color
614       params,
615       e_max_id
616       );   
617  }
618 
619} // namespace boost
620
621#endif // BOOST_GRAPH_SHOOTING_STAR_SEARCH_HPP
Note: See TracBrowser for help on using the browser.