pgRouting
pgRouting extends the PostGIS / PostgreSQL geospatial database to provide geospatial routing functionality.
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
src/BiDirAStar.cpp
Go to the documentation of this file.
1 /*PGR-MIT*****************************************************************
2 
3 * $Id$
4 *
5 * Project: pgRouting bdsp and bdastar algorithms
6 * Purpose:
7 * Author: Razequl Islam <ziboncsedu@gmail.com>
8 *
9 
10 ------
11 MIT/X license
12 
13 Permission is hereby granted, free of charge, to any person obtaining a copy
14 of this software and associated documentation files (the "Software"), to deal
15 in the Software without restriction, including without limitation the rights
16 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
17 copies of the Software, and to permit persons to whom the Software is
18 furnished to do so, subject to the following conditions:
19 
20 
21 The above copyright notice and this permission notice shall be included in
22 all copies or substantial portions of the Software.
23 
24 
25 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
26 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
27 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
28 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
29 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
30 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
31 THE SOFTWARE.
32 
33 ********************************************************************PGR-MIT*/
34 
35 #if defined(__MINGW32__) || defined(_MSC_VER)
36 #include <winsock2.h>
37 #include <windows.h>
38 #endif
39 
40 
41 #include <math.h>
42 #include "./BiDirAStar.h"
43 
45 }
46 
48 }
49 
51  // max_edge_id = 0;
52  // max_node_id = 0;
53 }
54 
55 /*
56  Initialization and allocation of memories.
57  */
58 
60  int i;
61 
62  // DBG("BiDirAStar::initall(%d) called\n", maxNode);
63 
64  m_pFParent = new PARENT_PATH[maxNode + 1];
65  m_pRParent = new PARENT_PATH[maxNode + 1];
66 
67  m_pFCost = new double[maxNode + 1];
68  m_pRCost = new double[maxNode + 1];
69 
70  for (i = 0; i <= maxNode; i++) {
71  m_pFParent[i].par_Node = -2;
72  m_pRParent[i].par_Node = -2;
73  m_pFCost[i] = INF;
74  m_pRCost[i] = INF;
75  }
76  m_MinCost = INF;
77  m_MidNode = -1;
78 
79  // reserve space for nodes and edges
80  m_vecNodeVector.reserve(maxNode + 1);
81 
82  // DBG("Leaving BiDirAStar::initall\n");
83 }
84 
85 /*
86  Delete the allocated memories to avoid memory leak.
87  */
88 
90  // DBG("Calling BiDirAStar::deleteall\n");
91  delete [] m_pFParent;
92  delete [] m_pRParent;
93  delete [] m_pFCost;
94  delete [] m_pRCost;
95  // DBG("Leaving BiDirAStar::deleteall\n");
96 }
97 
98 /*
99  Get the current cost from source to the current node if direction is 1 else the cost to reach target from the current node.
100  */
101 
102 double BiDirAStar::getcost(int node_id, int dir) {
103  if (dir == 1) {
104  return(m_pFCost[node_id]);
105  } else {
106  return(m_pRCost[node_id]);
107  }
108 }
109 
110 
111 double BiDirAStar::dist(double x1, double y1, double x2, double y2) {
112  double ret = fabs((x1 - x2) + fabs(y1 - y2));
113  // double ret = sqrt(x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
114  return(ret * 10);
115 }
116 
117 /*
118  Get the heuristic cost of the node depending on dir (1 for forward search and -1 for reverse search).
119  */
120 double BiDirAStar::gethcost(int node_id, int dir) {
121  if (dir == -1) {
122  return(dist(m_vecNodeVector[node_id].xpos, m_vecNodeVector[node_id].ypos, m_vecNodeVector[m_lStartNodeId].xpos, m_vecNodeVector[m_lStartNodeId].ypos));
123  } else {
124  return(dist(m_vecNodeVector[node_id].xpos, m_vecNodeVector[node_id].ypos, m_vecNodeVector[m_lEndNodeId].xpos, m_vecNodeVector[m_lEndNodeId].ypos));
125  }
126 }
127 
128 /*
129  Set the forward or reverse cost list depending on dir (1 for forward search and -1 for reverse search).
130  */
131 
132 
133 void BiDirAStar::setcost(int node_id, int dir, double c) {
134  if (dir == 1) {
135  m_pFCost[node_id] = c;
136  } else {
137  m_pRCost[node_id] = c;
138  }
139 }
140 
141 void BiDirAStar::setparent(int node_id, int dir, int parnode, int paredge) {
142  if (dir == 1) {
143  m_pFParent[node_id].par_Node = parnode;
144  m_pFParent[node_id].par_Edge = paredge;
145  } else {
146  m_pRParent[node_id].par_Node = parnode;
147  m_pRParent[node_id].par_Edge = paredge;
148  }
149 }
150 
151 /*
152  Reconstruct path for forward search. It is like normal dijkstra. The parent array contains the parent of the current node and there is a -1 in the source.
153  So one need to recurse up to the source and then add the current node and edge to the list.
154  */
155 
156 void BiDirAStar::fconstruct_path(int node_id) {
157  if (m_pFParent[node_id].par_Node == -1)
158  return;
159  fconstruct_path(m_pFParent[node_id].par_Node);
160  path_element_t pt;
161  pt.vertex_id = m_pFParent[node_id].par_Node;
162  pt.edge_id = m_pFParent[node_id].par_Edge;
163  pt.cost = m_pFCost[node_id] - m_pFCost[m_pFParent[node_id].par_Node];
164  m_vecPath.push_back(pt);
165 }
166 
167 /*
168  Reconstruct path for the reverse search. In this case the subsequent node is stored in the parent and the target contains a -1. So one need to add the node
169  and edge to the list and then recurse through the parent up to hitting a -1.
170  */
171 
172 void BiDirAStar::rconstruct_path(int node_id) {
173  path_element_t pt;
174  if (m_pRParent[node_id].par_Node == -1) {
175  pt.vertex_id = node_id;
176  pt.edge_id = -1;
177  pt.cost = 0.0;
178  return;
179  }
180  pt.vertex_id = node_id;
181  pt.cost = m_pRCost[node_id] - m_pRCost[m_pRParent[node_id].par_Node];
182  pt.edge_id = m_pRParent[node_id].par_Edge;
183  m_vecPath.push_back(pt);
184  rconstruct_path(m_pRParent[node_id].par_Node);
185 }
186 
187 /*
188  This is the main exploration module. The parameter dir indicates whether the exploration will be in forward or reverser direction. The reference to the corresponding
189  que is also passed as parameter que. The current node and the current costs are also available as parameter.
190  */
191 
192 // void BiDirAStar::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que)
193 void BiDirAStar::explore(int cur_node, double cur_cost, int dir, MinHeap &que) {
194  size_t i;
195  // Number of connected edges
196  auto con_edge = m_vecNodeVector[cur_node].Connected_Edges_Index.size();
197  double edge_cost;
198  for (i = 0; i < con_edge; i++) {
199  auto edge_index = m_vecNodeVector[cur_node].Connected_Edges_Index[i];
200  // Get the edge from the edge list.
201  GraphEdgeInfo edge = m_vecEdgeVector[edge_index];
202  // Get the connected node
203  int new_node = m_vecNodeVector[cur_node].Connected_Nodes[i];
204 #if 0 // mult is set but not used
205  int mult;
206 
207  if (edge.Direction == 0)
208  mult = 1;
209  else
210  mult = dir;
211 #endif
212  if (cur_node == edge.StartNode) {
213  // Current node is the startnode of the edge. For forward search it should use forward cost, otherwise it should use the reverse cost,
214  // i.e. if the reverse direction is valid then this node may be visited from the end node.
215  if (dir > 0)
216  edge_cost = edge.Cost;
217  else
218  edge_cost = edge.ReverseCost;
219  // Check if the direction is valid for exploration
220  if (edge.Direction == 0 || edge_cost >= 0.0) {
221  // edge_cost = edge.Cost * mult;
222  // Check if the current edge gives better result
223  if (cur_cost + edge_cost < getcost(new_node, dir)) {
224  // explore the node, and push it in the queue. the value in the queue will also contain the heuristic cost
225  setcost(new_node, dir, cur_cost + edge_cost);
226  setparent(new_node, dir, cur_node, edge.EdgeID);
227  que.push(std::make_pair(cur_cost + edge_cost + gethcost(new_node, dir), new_node));
228 
229  // Update the minimum cost found so far.
230  if (getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost) {
231  m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
232  m_MidNode = new_node;
233  }
234  }
235  }
236  } else {
237  // Current node is the endnode of the edge. For forward search it should use reverse cost, otherwise it should use the forward cost,
238  // i.e. if the forward direction is valid then this node may be visited from the start node.
239  if (dir > 0)
240  edge_cost = edge.ReverseCost;
241  else
242  edge_cost = edge.Cost;
243  // Check if the direction is valid for exploration
244  if (edge.Direction == 0 || edge_cost >= 0.0) {
245  // edge_cost = edge.ReverseCost * mult;
246 
247  // Check if the current edge gives better result
248  if (cur_cost + edge_cost < getcost(new_node, dir)) {
249  // explore the node, and push it in the queue. the value in the queue will also contain the heuristic cost
250  setcost(new_node, dir, cur_cost + edge_cost);
251  setparent(new_node, dir, cur_node, edge.EdgeID);
252  que.push(std::make_pair(cur_cost + edge_cost + gethcost(new_node, dir), new_node));
253  // Update the minimum cost found so far.
254  if (getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost) {
255  m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
256  m_MidNode = new_node;
257  }
258  }
259  }
260  }
261  }
262 }
263 
264 /*
265  This is the entry function that the wrappers should call. Most of the parameters are trivial. maxNode refers to Maximum
266  node id. As we run node based exploration cost, parent etc will be based on maximam node id.
267  */
268 
269 int BiDirAStar:: bidir_astar(edge_astar_t *edges, size_t edge_count, int maxNode, int start_vertex, int end_vertex,
270  path_element_t **path, size_t *path_count, char **err_msg) {
272  max_edge_id = -1;
273 
274  // Allocate memory for local storage like cost and parent holder
275  initall(maxNode);
276 
277  // construct the graph from the edge list, i.e. populate node and edge data structures
278  construct_graph(edges, edge_count, maxNode);
279 
280  m_lStartNodeId = start_vertex;
281  m_lEndNodeId = end_vertex;
282 
283  // int nodeCount = m_vecNodeVector.size();
284 
285  MinHeap fque(maxNode + 2);
286  MinHeap rque(maxNode + 2);
287  // std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > fque;
288  // std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > rque;
289 
290  m_vecPath.clear();
291 
292  // Initialize the forward search
293  m_pFParent[start_vertex].par_Node = -1;
294  m_pFParent[start_vertex].par_Edge = -1;
295  m_pFCost[start_vertex] = 0.0;
296  fque.push(std::make_pair(0.0, start_vertex));
297 
298  // Initialize the reverse search
299  m_pRParent[end_vertex].par_Node = -1;
300  m_pRParent[end_vertex].par_Edge = -1;
301  m_pRCost[end_vertex] = 0.0;
302  rque.push(std::make_pair(0.0, end_vertex));
303 
304  // int new_node;
305  int cur_node;
306  // int dir;
307  /*
308  The main loop. The algorithm is as follows:
309  1. IF the sum of the current minimum of both heap is greater than so far found path, we cannot get any better, so break the loop.
310  2. IF the reverse heap minimum is lower than the forward heap minimum, explore from reverse direction.
311  3. ELSE explore from the forward directtion.
312  */
313 
314  while (!fque.empty() && !rque.empty()) {
315  PDI fTop = fque.top();
316  PDI rTop = rque.top();
317  if (m_pFCost[fTop.second] + m_pRCost[rTop.second] > m_MinCost) // We are done, there is no path with lower cost
318  break;
319 
320  if (rTop.first < fTop.first) {
321  // Explore from reverse queue
322  if (rTop.first > m_MinCost)
323  break;
324  cur_node = rTop.second;
325  int dir = -1;
326  rque.pop();
327  explore(cur_node, m_pRCost[rTop.second], dir, rque);
328  } else {
329  // Explore from forward queue
330  if (fTop.first > m_MinCost)
331  break;
332  cur_node = fTop.second;
333  int dir = 1;
334  fque.pop();
335  explore(cur_node, m_pFCost[fTop.second], dir, fque);
336  }
337  }
338 
339  /*
340  Path reconstruction part. m_MidNode is the joining point where two searches meet to make a shortest path. It is updated in explore.
341  If it contains -1, then no path is found. Other wise we have a shortest path and that is reconstructed in the m_vecPath.
342  */
343 
344  if (m_MidNode == -1) {
345  *err_msg = (char *)"Path Not Found";
346  deleteall();
347  return -1;
348  } else {
349  // reconstruct path from forward search
351  // reconstruct path from backward search
353 
354  // insert the last row in the path trace (having edge_id = -1 and cost = 0.0)
355  path_element_t pelement;
356  pelement.vertex_id = end_vertex;
357  pelement.edge_id = -1;
358  pelement.cost = 0.0;
359  m_vecPath.push_back(pelement);
360 
361  // Transfer data path to path_element_t format and allocate memory and populate the pointer
362  *path = (path_element_t *) malloc(sizeof(path_element_t) * (m_vecPath.size() + 1));
363  *path_count = m_vecPath.size();
364 
365  for (size_t i = 0; i < *path_count; i++) {
366  (*path)[i].vertex_id = m_vecPath[i].vertex_id;
367  (*path)[i].edge_id = m_vecPath[i].edge_id;
368  (*path)[i].cost = m_vecPath[i].cost;
369  }
370  }
371  deleteall();
372  return 0;
373 }
374 
375 /*
376  Populate the member variables of the class using the edge list. Basically there is a node list and an edge list. Each node contains the list of adjacent nodes and
377  corresponding edge indices from edge list that connect this node with the adjacent nodes.
378  */
379 
381  int i;
382 
383  // DBG("Calling BiDirAStar::construct_graph(edges, ecnt:%d, maxNode:%d)\n", edge_count, maxNode);
384 
385  // Create a dummy node
386  GraphNodeInfo nodeInfo;
387  nodeInfo.Connected_Edges_Index.clear();
388  nodeInfo.Connected_Nodes.clear();
389 
390  // Insert the dummy node into the node list. This acts as place holder. Also change the nodeId so that nodeId and node index in the vector are same.
391  // There may be some nodes here that does not appear in the edge list. The size of the list is up to maxNode which is equal to maximum node id.
392  // DBG(" Adding nodes to m_vecNodeVector\n");
393  for (i = 0; i <= maxNode; i++) {
394  nodeInfo.NodeID = i;
395  m_vecNodeVector.push_back(nodeInfo);
396  }
397 
398  // Process each edge from the edge list and update the member data structures accordingly.
399  // DBG(" Reserving edges for graph(%d)\n", edge_count);
400  m_vecEdgeVector.reserve(edge_count);
401  // DBG(" Adding edges to graph\n");
402  for (size_t i = 0; i < edge_count; i++) {
403  addEdge(edges[i]);
404  }
405 
406  // DBG("Leaving BiDirAStar::construct_graph\n");
407  return true;
408 }
409 
410 /*
411  Process the edge and populate the member nodelist and edgelist. The nodelist already contains up to maxNode dummy entries with nodeId same as index. Now the
412  connectivity information needs to be updated.
413  */
414 
416  // long lTest;
417  // Check if the edge is already processed.
418  Long2LongMap::iterator itMap = m_mapEdgeId2Index.find(edgeIn.id);
419  if (itMap != m_mapEdgeId2Index.end())
420  return false;
421 
422  // Create a GraphEdgeInfo using the information of the current edge
423  GraphEdgeInfo newEdge;
424  newEdge.EdgeID = edgeIn.id;
425  newEdge.EdgeIndex = m_vecEdgeVector.size();
426  newEdge.StartNode = edgeIn.source;
427  newEdge.EndNode = edgeIn.target;
428  newEdge.Cost = edgeIn.cost;
429  newEdge.ReverseCost = edgeIn.reverse_cost;
430 
431  // Set the direction. If both cost and reverse cost has positive value the edge is bidirectional and direction field is 0. If cost is positive and reverse cost
432  // negative then the edge is unidirectional with direction = 1 (goes from source to target) otherwise it is unidirectional with direction = -1 (goes from target
433  // to source). Another way of creating unidirectional edge is assigning big values in cost or reverse_cost. In that case the direction is still zero and this case
434  // is handled in the algorithm automatically.
435  if (newEdge.Cost >= 0.0 && newEdge.ReverseCost >= 0) {
436  newEdge.Direction = 0;
437  } else if (newEdge.Cost >= 0.0) {
438  newEdge.Direction = 1;
439  } else {
440  newEdge.Direction = -1;
441  }
442 
443  if (edgeIn.id > max_edge_id) {
444  max_edge_id = edgeIn.id;
445  }
446 
447  // Update max_edge_id
448  if (newEdge.StartNode > max_node_id) {
449  return false; // max_node_id = newEdge.StartNode;
450  }
451  if (newEdge.EndNode > max_node_id) {
452  return false; // max_node_id = newEdge.EdgeIndex;
453  }
454 
455  m_vecNodeVector[newEdge.StartNode].xpos = edgeIn.s_x;
456  m_vecNodeVector[newEdge.StartNode].ypos = edgeIn.s_y;
457 
458  m_vecNodeVector[newEdge.EndNode].xpos = edgeIn.t_x;
459  m_vecNodeVector[newEdge.EndNode].ypos = edgeIn.t_y;
460 
461  // update connectivity information for the start node.
462  m_vecNodeVector[newEdge.StartNode].Connected_Nodes.push_back(newEdge.EndNode);
463  m_vecNodeVector[newEdge.StartNode].Connected_Edges_Index.push_back(newEdge.EdgeIndex);
464 
465  // update connectivity information for the end node.
466  m_vecNodeVector[newEdge.EndNode].Connected_Nodes.push_back(newEdge.StartNode);
467  m_vecNodeVector[newEdge.EndNode].Connected_Edges_Index.push_back(newEdge.EdgeIndex);
468 
469 
470 
471  // Adding edge to the list
472  m_mapEdgeId2Index.insert(std::make_pair(newEdge.EdgeID, m_vecEdgeVector.size()));
473  m_vecEdgeVector.push_back(newEdge);
474 
475  return true;
476 }
PARENT_PATH * m_pFParent
PDI top()
Definition: src/MinHeap.cpp:88
double cost
double m_MinCost
double * m_pRCost
int path_count
Definition: BDATester.cpp:51
void pop()
Definition: src/MinHeap.cpp:98
std::vector< size_t > Connected_Edges_Index
bool construct_graph(edge_astar_t *edges, size_t edge_count, int maxNode)
PARENT_PATH * m_pRParent
std::pair< double, int > PDI
#define INF
double ReverseCost
std::vector< int > Connected_Nodes
double s_y
int bidir_astar(edge_astar_t *edges, size_t edge_count, int maxNode, int start_vertex, int end_vertex, path_element_t **path, size_t *path_count, char **err_msg)
double gethcost(int node_id, int dir)
void setcost(int node_id, int dir, double c)
GraphNodeVector m_vecNodeVector
int maxNode
Definition: BDATester.cpp:47
void explore(int cur_node, double cur_cost, int dir, MinHeap &que)
int edge_count
Definition: BDATester.cpp:47
std::vector< path_element_t > m_vecPath
edge_astar_t * edges
Definition: BDATester.cpp:46
double s_x
double t_y
bool empty()
Definition: src/MinHeap.cpp:92
double getcost(int node_id, int dir)
GraphEdgeVector m_vecEdgeVector
void push(PDI node)
Definition: src/MinHeap.cpp:69
void deleteall()
int64_t edge_id
Definition: pgr_types.h:75
void fconstruct_path(int node_id)
double * m_pFCost
path_element_t * path
Definition: BDATester.cpp:49
void setparent(int node_id, int dir, int parnode, int paredge)
int64_t vertex_id
Definition: pgr_types.h:74
double dist(double x1, double y1, double x2, double y2)
char * err_msg
Definition: BDATester.cpp:50
Long2LongMap m_mapEdgeId2Index
void rconstruct_path(int node_id)
bool addEdge(edge_astar_t edgeIn)
double t_x
double reverse_cost
void initall(int maxNode)
double cost
Definition: pgr_types.h:76