#
root/trunk/core/src/astar_boost_wrapper.cpp

Revision 178, 7.1 KB (checked in by anton, 2 years ago) |
---|

Line | |
---|---|

1 | /* |

2 | * A* Shortest path algorithm for PostgreSQL |

3 | * |

4 | * Copyright (c) 2006 Anton A. Patrushev, Orkney, Inc. |

5 | * |

6 | * This program is free software; you can redistribute it and/or modify |

7 | * it under the terms of the GNU General Public License as published by |

8 | * the Free Software Foundation; either version 2 of the License, or |

9 | * (at your option) any later version. |

10 | * |

11 | * This program is distributed in the hope that it will be useful, |

12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |

13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |

14 | * GNU General Public License for more details. |

15 | * |

16 | * You should have received a copy of the GNU General Public License |

17 | * along with this program; if not, write to the Free Software |

18 | * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |

19 | * |

20 | */ |

21 | |

22 | #include <boost/config.hpp> |

23 | |

24 | #include <boost/graph/graph_traits.hpp> |

25 | #include <boost/graph/adjacency_list.hpp> |

26 | #include <boost/graph/astar_search.hpp> |

27 | |

28 | #include "astar.h" |

29 | |

30 | #include <cmath> // for sqrt |

31 | |

32 | using namespace std; |

33 | using namespace boost; |

34 | |

35 | // Maximal number of nodes in the path (to avoid infinite loops) |

36 | #define MAX_NODES 100000000 |

37 | |

38 | struct Edge |

39 | { |

40 | int id; |

41 | float8 cost; |

42 | //float8 distance; |

43 | }; |

44 | |

45 | struct Vertex |

46 | { |

47 | int id; |

48 | float8 x; |

49 | float8 y; |

50 | }; |

51 | |

52 | |

53 | struct found_goal {}; // exception for termination |

54 | |

55 | // visitor that terminates when we find the goal |

56 | template <class Vertex> |

57 | class astar_goal_visitor : public boost::default_astar_visitor |

58 | { |

59 | public: |

60 | astar_goal_visitor(Vertex goal) : m_goal(goal) {} |

61 | template <class Graph> |

62 | void examine_vertex(Vertex u, Graph& g) { |

63 | if(u == m_goal) |

64 | throw found_goal(); |

65 | } |

66 | private: |

67 | Vertex m_goal; |

68 | }; |

69 | |

70 | // Heuristic function which tells us how far the current node is |

71 | // from the target node. |

72 | // |

73 | // (|dx|+|dy|)/2 is used currently. |

74 | template <class Graph, class CostType> |

75 | class distance_heuristic : public astar_heuristic<Graph, CostType> |

76 | { |

77 | public: |

78 | typedef typename graph_traits<Graph>::vertex_descriptor Vertex; |

79 | distance_heuristic(Graph& g, Vertex goal):m_g(g), m_goal(goal){} |

80 | CostType operator()(Vertex u) |

81 | { |

82 | CostType dx = m_g[m_goal].x - m_g[u].x; |

83 | CostType dy = m_g[m_goal].y - m_g[u].y; |

84 | //You can choose any heuristical function from below |

85 | //return ::max(dx, dy); |

86 | //return ::sqrt(dx * dx + dy * dy)/4; |

87 | //return 0; |

88 | return (::fabs(dx)+::fabs(dy))/2; |

89 | } |

90 | private: |

91 | Graph& m_g; |

92 | Vertex m_goal; |

93 | }; |

94 | |

95 | |

96 | // Adds an edge to the graph. |

97 | // Edge id, cost, source and target ids and coordinates are copied also |

98 | template <class G, class E> |

99 | static void |

100 | graph_add_edge(G &graph, int id, int source, int target, |

101 | float8 cost, float8 s_x, float8 s_y, float8 t_x, float8 t_y) |

102 | { |

103 | E e; |

104 | bool inserted; |

105 | |

106 | if (cost < 0) // edges are not inserted in the graph if cost is negative |

107 | return; |

108 | |

109 | tie(e, inserted) = add_edge(source, target, graph); |

110 | |

111 | graph[e].cost = cost; |

112 | graph[e].id = id; |

113 | |

114 | typedef typename graph_traits<G>::vertex_descriptor Vertex; |

115 | Vertex s = vertex(source, graph); |

116 | Vertex t = vertex(target, graph); |

117 | graph[s].x=s_x; |

118 | graph[s].y=s_y; |

119 | graph[t].x=t_x; |

120 | graph[t].y=t_y; |

121 | } |

122 | |

123 | int |

124 | boost_astar(edge_astar_t *edges, unsigned int count, |

125 | int source_vertex_id, int target_vertex_id, |

126 | bool directed, bool has_reverse_cost, |

127 | path_element_t **path, int *path_count, char **err_msg) |

128 | { |

129 | |

130 | // FIXME: use a template for the directedS parameters |

131 | typedef adjacency_list < listS, vecS, directedS, Vertex, Edge> graph_t; |

132 | |

133 | typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor; |

134 | typedef graph_traits < graph_t >::edge_descriptor edge_descriptor; |

135 | |

136 | // FIXME: compute this value |

137 | const unsigned int num_nodes = ((directed && has_reverse_cost ? 2 : 1) * |

138 | count) + 100; |

139 | |

140 | graph_t graph(num_nodes); |

141 | |

142 | property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, |

143 | graph); |

144 | |

145 | for (std::size_t j = 0; j < count; ++j) |

146 | { |

147 | |

148 | graph_add_edge<graph_t, edge_descriptor>(graph, |

149 | edges[j].id, edges[j].source, |

150 | edges[j].target, edges[j].cost, |

151 | edges[j].s_x, edges[j].s_y, |

152 | edges[j].t_x, edges[j].t_y); |

153 | |

154 | if (!directed || (directed && has_reverse_cost)) |

155 | { |

156 | float8 cost; |

157 | |

158 | if (has_reverse_cost) |

159 | { |

160 | cost = edges[j].reverse_cost; |

161 | } |

162 | else |

163 | { |

164 | cost = edges[j].cost; |

165 | } |

166 | |

167 | graph_add_edge<graph_t, edge_descriptor>(graph, |

168 | edges[j].id, |

169 | edges[j].target, |

170 | edges[j].source, |

171 | cost, |

172 | edges[j].s_x, |

173 | edges[j].s_y, |

174 | edges[j].t_x, |

175 | edges[j].t_y); |

176 | } |

177 | } |

178 | |

179 | std::vector<vertex_descriptor> predecessors(num_vertices(graph)); |

180 | |

181 | vertex_descriptor source_vertex = vertex(source_vertex_id, graph); |

182 | |

183 | if (source_vertex < 0) |

184 | { |

185 | *err_msg = (char *) "Source vertex not found"; |

186 | return -1; |

187 | } |

188 | |

189 | vertex_descriptor target_vertex = vertex(target_vertex_id, graph); |

190 | if (target_vertex < 0) |

191 | { |

192 | *err_msg = (char *) "Target vertex not found"; |

193 | return -1; |

194 | } |

195 | |

196 | std::vector<float8> distances(num_vertices(graph)); |

197 | |

198 | try { |

199 | // Call A* named parameter interface |

200 | astar_search |

201 | (graph, source_vertex, |

202 | distance_heuristic<graph_t, float>(graph, target_vertex), |

203 | predecessor_map(&predecessors[0]). |

204 | weight_map(get(&Edge::cost, graph)). |

205 | distance_map(&distances[0]). |

206 | visitor(astar_goal_visitor<vertex_descriptor>(target_vertex))); |

207 | |

208 | } |

209 | catch(found_goal fg) { |

210 | // Target vertex found |

211 | vector<int> path_vect; |

212 | int max = MAX_NODES; |

213 | path_vect.push_back(target_vertex); |

214 | |

215 | while (target_vertex != source_vertex) |

216 | { |

217 | if (target_vertex == predecessors[target_vertex]) |

218 | { |

219 | *err_msg = (char *) "No path found"; |

220 | return 0; |

221 | |

222 | } |

223 | target_vertex = predecessors[target_vertex]; |

224 | |

225 | path_vect.push_back(target_vertex); |

226 | if (!max--) |

227 | { |

228 | *err_msg = (char *) "Overflow"; |

229 | return -1; |

230 | } |

231 | } |

232 | |

233 | *path = (path_element_t *) malloc(sizeof(path_element_t) * |

234 | (path_vect.size() + 1)); |

235 | *path_count = path_vect.size(); |

236 | |

237 | for(int i = path_vect.size() - 1, j = 0; i >= 0; i--, j++) |

238 | { |

239 | graph_traits < graph_t >::vertex_descriptor v_src; |

240 | graph_traits < graph_t >::vertex_descriptor v_targ; |

241 | graph_traits < graph_t >::edge_descriptor e; |

242 | graph_traits < graph_t >::out_edge_iterator out_i, out_end; |

243 | |

244 | (*path)[j].vertex_id = path_vect.at(i); |

245 | |

246 | (*path)[j].edge_id = -1; |

247 | (*path)[j].cost = distances[target_vertex]; |

248 | |

249 | if (i == 0) |

250 | { |

251 | continue; |

252 | } |

253 | |

254 | v_src = path_vect.at(i); |

255 | v_targ = path_vect.at(i - 1); |

256 | |

257 | for (tie(out_i, out_end) = out_edges(v_src, graph); |

258 | out_i != out_end; ++out_i) |

259 | { |

260 | graph_traits < graph_t >::vertex_descriptor v, targ; |

261 | e = *out_i; |

262 | v = source(e, graph); |

263 | targ = target(e, graph); |

264 | |

265 | if (targ == v_targ) |

266 | { |

267 | (*path)[j].edge_id = graph[*out_i].id; |

268 | (*path)[j].cost = graph[*out_i].cost; |

269 | break; |

270 | } |

271 | } |

272 | } |

273 | |

274 | return EXIT_SUCCESS; |

275 | } |

276 | } |

277 |

**Note:**See TracBrowser for help on using the browser.