root/trunk/core/src/astar_boost_wrapper.cpp

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

constant string conversion fixed

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
32using namespace std;
33using namespace boost;
34
35// Maximal number of nodes in the path (to avoid infinite loops)
36#define MAX_NODES 100000000
37
38struct Edge
39{
40  int id;
41  float8 cost;
42  //float8 distance;
43};
44       
45struct Vertex
46{
47  int id;
48  float8 x;
49  float8 y;
50};
51
52
53struct found_goal {}; // exception for termination
54
55// visitor that terminates when we find the goal
56template <class Vertex>
57class astar_goal_visitor : public boost::default_astar_visitor
58{
59public:
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  }
66private:
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.
74template <class Graph, class CostType>
75class distance_heuristic : public astar_heuristic<Graph, CostType>
76{
77public:
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  }
90private:
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
98template <class G, class E>
99static void
100graph_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
123int
124boost_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.