28 template <
typename Graph,
typename AStarHeuristic,
29 typename LPAStarVisitor,
typename PredecessorMap,
30 typename DistanceMap,
typename DistanceLookaheadMap,
31 typename WeightMap,
typename VertexIndexMap,
32 typename CompareFunction,
typename CombineFunction,
33 typename CostInf,
typename CostZero>
37 typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
38 typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIter;
39 typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
40 typedef typename boost::graph_traits<Graph>::out_edge_iterator OutEdgeIter;
41 typedef typename boost::graph_traits<Graph>::in_edge_iterator InEdgeIter;
42 typedef typename boost::property_traits<WeightMap>::value_type weight_type;
49 PredecessorMap predecessor;
51 DistanceLookaheadMap distance_lookahead;
53 VertexIndexMap index_map;
54 CompareFunction compare;
55 CombineFunction combine;
58 weight_type goal_margin;
64 Vertex v_start, Vertex v_goal,
67 PredecessorMap predecessor,
68 DistanceMap distance, DistanceLookaheadMap distance_lookahead,
70 VertexIndexMap index_map,
71 CompareFunction compare, CombineFunction combine,
72 CostInf inf, CostZero zero,
73 weight_type goal_margin):
74 g(g), v_start(v_start), v_goal(v_goal),
75 h(h), vis(vis), predecessor(predecessor),
76 distance(distance), distance_lookahead(distance_lookahead),
77 weight(weight), index_map(index_map),
78 compare(compare), combine(combine),
80 goal_margin(goal_margin)
87 VertexIter vi, vi_end;
88 for (boost::tie(vi,vi_end)=vertices(g); vi!=vi_end; ++vi)
90 put(distance_lookahead, *vi, inf);
91 put(distance, *vi, inf);
93 put(predecessor, v_start, v_start);
94 put(distance_lookahead, v_start, zero);
96 queue.insert(
get(index_map,v_start), std::make_pair(h(v_start),0));
99 inline std::pair<weight_type,weight_type> calculate_key(Vertex u,
bool do_goal_margin=
false)
102 = std::min(
get(distance,u),
get(distance_lookahead,u));
104 minval += goal_margin;
105 return std::make_pair(minval+h(u), minval);
114 inline bool update_predecessor(Vertex u, Vertex v, weight_type uv_weight)
121 Vertex v_pred =
get(predecessor, v);
122 weight_type v_look_old =
get(distance_lookahead, v);
123 weight_type v_look_u = combine(
get(distance,u), uv_weight);
128 if (v_look_u == v_look_old)
132 else if (v_look_u < v_look_old)
134 put(distance_lookahead, v, v_look_u);
140 weight_type v_look_best = inf;
142 InEdgeIter ei, ei_end;
143 for (boost::tie(ei,ei_end)=in_edges(v,g); ei!=ei_end; ei++)
145 weight_type v_look_uu = combine(
get(distance,source(*ei,g)),
get(weight,*ei));
146 if (v_look_uu < v_look_best)
148 v_look_best = v_look_uu;
149 v_pred_best = source(*ei,g);
152 if (v_look_best != inf)
153 put(predecessor, v, v_pred_best);
154 if (v_look_best == v_look_old)
160 put(distance_lookahead, v, v_look_best);
167 if (v_look_u < v_look_old)
169 put(predecessor, v, u);
170 put(distance_lookahead, v, v_look_u);
183 inline void update_vertex(Vertex u)
185 weight_type u_dist =
get(distance,u);
186 bool is_consistent = (u_dist ==
get(distance_lookahead,u));
187 size_t u_idx =
get(index_map,u);
190 if (queue.contains(u_idx))
195 if (queue.contains(u_idx))
196 queue.update(u_idx, calculate_key(u));
198 queue.insert(u_idx, calculate_key(u));
202 void compute_shortest_path()
205 && (queue.top_key() < calculate_key(v_goal,
true)
206 ||
get(distance_lookahead,v_goal) !=
get(distance,v_goal)))
208 Vertex u = vertex(queue.top_idx(), g);
210 vis.examine_vertex(u, g);
212 if (
get(distance,u) >
get(distance_lookahead,u))
214 put(distance, u,
get(distance_lookahead,u));
215 OutEdgeIter ei, ei_end;
216 for (boost::tie(ei,ei_end)=out_edges(u,g); ei!=ei_end; ei++)
218 Vertex v = target(*ei,g);
219 bool lookahead_changed = update_predecessor(u, v,
get(weight,*ei));
220 if (lookahead_changed)
226 put(distance, u, inf);
228 OutEdgeIter ei, ei_end;
229 for (boost::tie(ei,ei_end)=out_edges(u,g); ei!=ei_end; ei++)
231 Vertex v = target(*ei,g);
232 bool lookahead_changed = update_predecessor(u, v,
get(weight,*ei));
233 if (lookahead_changed)
Binary min-heap with index lookups.
Definition: heap_indexed.h:34
Implements the Lifelong Planning A* incremental search algorithm.
Definition: lpastar.h:34