pgRouting  2.2
pgRouting extends the PostGIS / PostgreSQL geospatial database to provide geospatial routing functionality.
 All Classes Functions Variables Pages
src/BiDirAStar.cpp
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 #ifdef __MINGW32__
36 #include <winsock2.h>
37 #include <windows.h>
38 #endif
39 
40 
41 #include "BiDirAStar.h"
42 
43 BiDirAStar::BiDirAStar(void)
44 {
45 }
46 
47 BiDirAStar::~BiDirAStar(void)
48 {
49 }
50 
51 void BiDirAStar::init()
52 {
53  //max_edge_id = 0;
54  //max_node_id = 0;
55 
56 }
57 
58 /*
59  Initialization and allocation of memories.
60 */
61 
62 void BiDirAStar::initall(int maxNode)
63 {
64  int i;
65 
66  // DBG("BiDirAStar::initall(%d) called\n", maxNode);
67 
68  m_pFParent = new PARENT_PATH[maxNode + 1];
69  m_pRParent = new PARENT_PATH[maxNode + 1];
70 
71  m_pFCost = new double[maxNode + 1];
72  m_pRCost = new double[maxNode + 1];
73 
74  for(i = 0; i <= maxNode; i++)
75  {
76  m_pFParent[i].par_Node = -2;
77  m_pRParent[i].par_Node = -2;
78  m_pFCost[i] = INF;
79  m_pRCost[i] = INF;
80 
81  }
82  m_MinCost = INF;
83  m_MidNode = -1;
84 
85  // reserve space for nodes and edges
86  m_vecNodeVector.reserve(maxNode + 1);
87 
88  // DBG("Leaving BiDirAStar::initall\n");
89 }
90 
91 /*
92  Delete the allocated memories to avoid memory leak.
93 */
94 
95 void BiDirAStar::deleteall()
96 {
97  // DBG("Calling BiDirAStar::deleteall\n");
98  delete [] m_pFParent;
99  delete [] m_pRParent;
100  delete [] m_pFCost;
101  delete [] m_pRCost;
102  // DBG("Leaving BiDirAStar::deleteall\n");
103 }
104 
105 /*
106  Get the current cost from source to the current node if direction is 1 else the cost to reach target from the current node.
107 */
108 
109 double BiDirAStar::getcost(int node_id, int dir)
110 {
111  if(dir == 1)
112  {
113  return(m_pFCost[node_id]);
114  }
115  else
116  {
117  return(m_pRCost[node_id]);
118  }
119 }
120 
121 
122 double BiDirAStar::dist(double x1, double y1, double x2, double y2)
123 {
124  double ret = fabs((x1 - x2) + fabs(y1 - y2));
125  //double ret = sqrt(x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
126  return(ret * 10);
127 }
128 
129 /*
130  Get the heuristic cost of the node depending on dir (1 for forward search and -1 for reverse search).
131 */
132 double BiDirAStar::gethcost(int node_id, int dir)
133 {
134  if(dir == -1)
135  {
136  return(dist(m_vecNodeVector[node_id].xpos, m_vecNodeVector[node_id].ypos, m_vecNodeVector[m_lStartNodeId].xpos, m_vecNodeVector[m_lStartNodeId].ypos));
137  }
138  else
139  {
140  return(dist(m_vecNodeVector[node_id].xpos, m_vecNodeVector[node_id].ypos, m_vecNodeVector[m_lEndNodeId].xpos, m_vecNodeVector[m_lEndNodeId].ypos));
141  }
142 }
143 
144 /*
145  Set the forward or reverse cost list depending on dir (1 for forward search and -1 for reverse search).
146 */
147 
148 
149 void BiDirAStar::setcost(int node_id, int dir, double c)
150 {
151  if(dir == 1)
152  {
153  m_pFCost[node_id] = c;
154  }
155  else
156  {
157  m_pRCost[node_id] = c;
158  }
159 }
160 
161 void BiDirAStar::setparent(int node_id, int dir, int parnode, int paredge)
162 {
163  if(dir == 1)
164  {
165  m_pFParent[node_id].par_Node = parnode;
166  m_pFParent[node_id].par_Edge = paredge;
167  }
168  else
169  {
170  m_pRParent[node_id].par_Node = parnode;
171  m_pRParent[node_id].par_Edge = paredge;
172  }
173 }
174 
175 /*
176  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.
177  So one need to recurse upto the source and then add the current node and edge to the list.
178 */
179 
180 void BiDirAStar::fconstruct_path(int node_id)
181 {
182  if(m_pFParent[node_id].par_Node == -1)
183  return;
184  fconstruct_path(m_pFParent[node_id].par_Node);
185  path_element_t pt;
186  pt.vertex_id = m_pFParent[node_id].par_Node;
187  pt.edge_id = m_pFParent[node_id].par_Edge;
188  pt.cost = m_pFCost[node_id] - m_pFCost[m_pFParent[node_id].par_Node];
189  m_vecPath.push_back(pt);
190 }
191 
192 /*
193  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
194  and edge to the list and then recurse through the parent upto hitting a -1.
195 */
196 
197 void BiDirAStar::rconstruct_path(int node_id)
198 {
199  path_element_t pt;
200  if(m_pRParent[node_id].par_Node == -1)
201  {
202  pt.vertex_id = node_id;
203  pt.edge_id = -1;
204  pt.cost = 0.0;
205  return;
206  }
207  pt.vertex_id = node_id;
208  pt.cost = m_pRCost[node_id] - m_pRCost[m_pRParent[node_id].par_Node];
209  pt.edge_id = m_pRParent[node_id].par_Edge;
210  m_vecPath.push_back(pt);
211  rconstruct_path(m_pRParent[node_id].par_Node);
212 }
213 
214 /*
215  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
216  que is also passed as parameter que. The current node and the current costs are also available as parameter.
217 */
218 
219 //void BiDirAStar::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que)
220 void BiDirAStar::explore(int cur_node, double cur_cost, int dir, MinHeap &que)
221 {
222  size_t i;
223  // Number of connected edges
224  auto con_edge = m_vecNodeVector[cur_node].Connected_Edges_Index.size();
225  double edge_cost;
226  for(i = 0; i < con_edge; i++)
227  {
228  auto edge_index = m_vecNodeVector[cur_node].Connected_Edges_Index[i];
229  // Get the edge from the edge list.
230  GraphEdgeInfo edge = m_vecEdgeVector[edge_index];
231  // Get the connected node
232  int new_node = m_vecNodeVector[cur_node].Connected_Nodes[i];
233 #if 0 // mult is set but not used
234  int mult;
235 
236  if(edge.Direction == 0)
237  mult = 1;
238  else
239  mult = dir;
240 #endif
241  if(cur_node == edge.StartNode)
242  {
243  // Current node is the startnode of the edge. For forward search it should use forward cost, otherwise it should use the reverse cost,
244  // i.e. if the reverse direction is valid then this node may be visited from the end node.
245  if(dir > 0)
246  edge_cost = edge.Cost;
247  else
248  edge_cost = edge.ReverseCost;
249  // Check if the direction is valid for exploration
250  if(edge.Direction == 0 || edge_cost >= 0.0)
251  {
252  //edge_cost = edge.Cost * mult;
253  // Check if the current edge gives better result
254  if(cur_cost + edge_cost < getcost(new_node, dir))
255  {
256  // explore the node, and push it in the queue. the value in the queue will also contain the heuristic cost
257  setcost(new_node, dir, cur_cost + edge_cost);
258  setparent(new_node, dir, cur_node, edge.EdgeID);
259  que.push(std::make_pair(cur_cost + edge_cost + gethcost(new_node, dir), new_node));
260 
261  // Update the minimum cost found so far.
262  if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
263  {
264  m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
265  m_MidNode = new_node;
266  }
267  }
268  }
269  }
270  else
271  {
272  // Current node is the endnode of the edge. For forward search it should use reverse cost, otherwise it should use the forward cost,
273  // i.e. if the forward direction is valid then this node may be visited from the start node.
274  if(dir > 0)
275  edge_cost = edge.ReverseCost;
276  else
277  edge_cost = edge.Cost;
278  // Check if the direction is valid for exploration
279  if(edge.Direction == 0 || edge_cost >= 0.0)
280  {
281  //edge_cost = edge.ReverseCost * mult;
282 
283  // Check if the current edge gives better result
284  if(cur_cost + edge_cost < getcost(new_node, dir))
285  {
286  // explore the node, and push it in the queue. the value in the queue will also contain the heuristic cost
287  setcost(new_node, dir, cur_cost + edge_cost);
288  setparent(new_node, dir, cur_node, edge.EdgeID);
289  que.push(std::make_pair(cur_cost + edge_cost + gethcost(new_node, dir), new_node));
290  // Update the minimum cost found so far.
291  if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
292  {
293  m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
294  m_MidNode = new_node;
295  }
296  }
297  }
298  }
299  }
300 }
301 
302 /*
303  This is the entry function that the wrappers should call. Most of the parameters are trivial. maxNode refers to Maximum
304  node id. As we run node based exploration cost, parent etc will be based on maximam node id.
305 */
306 
307 int BiDirAStar:: bidir_astar(edge_astar_t *edges, size_t edge_count, int maxNode, int start_vertex, int end_vertex,
308  path_element_t **path, size_t *path_count, char **err_msg)
309 {
310  max_node_id = maxNode;
311  max_edge_id = -1;
312 
313  // Allocate memory for local storage like cost and parent holder
314  initall(maxNode);
315 
316  // construct the graph from the edge list, i.e. populate node and edge data structures
317  construct_graph(edges, edge_count, maxNode);
318 
319  m_lStartNodeId = start_vertex;
320  m_lEndNodeId = end_vertex;
321 
322  // int nodeCount = m_vecNodeVector.size();
323 
324  MinHeap fque(maxNode + 2);
325  MinHeap rque(maxNode + 2);
326  //std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > fque;
327  //std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > rque;
328 
329  m_vecPath.clear();
330 
331  // Initialize the forward search
332  m_pFParent[start_vertex].par_Node = -1;
333  m_pFParent[start_vertex].par_Edge = -1;
334  m_pFCost[start_vertex] = 0.0;
335  fque.push(std::make_pair(0.0, start_vertex));
336 
337  // Initialize the reverse search
338  m_pRParent[end_vertex].par_Node = -1;
339  m_pRParent[end_vertex].par_Edge = -1;
340  m_pRCost[end_vertex] = 0.0;
341  rque.push(std::make_pair(0.0, end_vertex));
342 
343  // int new_node;
344  int cur_node;
345  // int dir;
346 /*
347  The main loop. The algorithm is as follows:
348  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.
349  2. IF the reverse heap minimum is lower than the forward heap minimum, explore from reverse direction.
350  3. ELSE explore from the forward directtion.
351 */
352 
353  while(!fque.empty() && !rque.empty())
354  {
355  PDI fTop = fque.top();
356  PDI rTop = rque.top();
357  if(m_pFCost[fTop.second] + m_pRCost[rTop.second] > m_MinCost) //We are done, there is no path with lower cost
358  break;
359 
360  if(rTop.first < fTop.first) // Explore from reverse queue
361  {
362  if(rTop.first > m_MinCost)
363  break;
364  cur_node = rTop.second;
365  int dir = -1;
366  rque.pop();
367  explore(cur_node, m_pRCost[rTop.second], dir, rque);
368  }
369  else // Explore from forward queue
370  {
371  if(fTop.first > m_MinCost)
372  break;
373  cur_node = fTop.second;
374  int dir = 1;
375  fque.pop();
376  explore(cur_node, m_pFCost[fTop.second], dir, fque);
377  }
378  }
379 
380 /*
381  Path reconstruction part. m_MidNode is the joining point where two searches meet to make a shortest path. It is updated in explore.
382  If it contains -1, then no path is found. Other wise we have a shortest path and that is reconstructed in the m_vecPath.
383 */
384 
385  if(m_MidNode == -1)
386  {
387  *err_msg = (char *)"Path Not Found";
388  deleteall();
389  return -1;
390  }
391  else
392  {
393  // reconstruct path from forward search
394  fconstruct_path(m_MidNode);
395  // reconstruct path from backward search
396  rconstruct_path(m_MidNode);
397 
398  // insert the last row in the path trace (having edge_id = -1 and cost = 0.0)
399  path_element_t pelement;
400  pelement.vertex_id = end_vertex;
401  pelement.edge_id = -1;
402  pelement.cost = 0.0;
403  m_vecPath.push_back(pelement);
404 
405  // Transfer data path to path_element_t format and allocate memory and populate the pointer
406  *path = (path_element_t *) malloc(sizeof(path_element_t) * (m_vecPath.size() + 1));
407  *path_count = m_vecPath.size();
408 
409  for(size_t i = 0; i < *path_count; i++)
410  {
411  (*path)[i].vertex_id = m_vecPath[i].vertex_id;
412  (*path)[i].edge_id = m_vecPath[i].edge_id;
413  (*path)[i].cost = m_vecPath[i].cost;
414  }
415 
416  }
417  deleteall();
418  return 0;
419 }
420 
421 /*
422  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
423  corresponding edge indices from edge list that connect this node with the adjacent nodes.
424 */
425 
426 bool BiDirAStar::construct_graph(edge_astar_t* edges, size_t edge_count, int maxNode)
427 {
428  int i;
429 
430  // DBG("Calling BiDirAStar::construct_graph(edges, ecnt:%d, maxNode:%d)\n", edge_count, maxNode);
431 
432  // Create a dummy node
433  GraphNodeInfo nodeInfo;
434  nodeInfo.Connected_Edges_Index.clear();
435  nodeInfo.Connected_Nodes.clear();
436 
437  // 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.
438  // There may be some nodes here that does not appear in the edge list. The size of the list is upto maxNode which is equal to maximum node id.
439  // DBG(" Adding nodes to m_vecNodeVector\n");
440  for(i = 0; i <= maxNode; i++)
441  {
442  nodeInfo.NodeID = i;
443  m_vecNodeVector.push_back(nodeInfo);
444  }
445 
446  // Process each edge from the edge list and update the member data structures accordingly.
447  // DBG(" Reserving edges for graph(%d)\n", edge_count);
448  m_vecEdgeVector.reserve(edge_count);
449  // DBG(" Adding edges to graph\n");
450  for(size_t i = 0; i < edge_count; i++)
451  {
452  addEdge(edges[i]);
453  }
454 
455  // DBG("Leaving BiDirAStar::construct_graph\n");
456  return true;
457 }
458 
459 /*
460  Process the edge and populate the member nodelist and edgelist. The nodelist already contains upto maxNode dummy entries with nodeId same as index. Now the
461  connectivity information needs to be updated.
462 */
463 
464 bool BiDirAStar::addEdge(edge_astar_t edgeIn)
465 {
466  // long lTest;
467  // Check if the edge is already processed.
468  Long2LongMap::iterator itMap = m_mapEdgeId2Index.find(edgeIn.id);
469  if(itMap != m_mapEdgeId2Index.end())
470  return false;
471 
472  // Create a GraphEdgeInfo using the information of the current edge
473  GraphEdgeInfo newEdge;
474  newEdge.EdgeID = edgeIn.id;
475  newEdge.EdgeIndex = m_vecEdgeVector.size();
476  newEdge.StartNode = edgeIn.source;
477  newEdge.EndNode = edgeIn.target;
478  newEdge.Cost = edgeIn.cost;
479  newEdge.ReverseCost = edgeIn.reverse_cost;
480 
481  // 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
482  // negative then the edge is unidirectional with direction = 1 (goes from source to target) otherwise it is unidirectional with direction = -1 (goes from target
483  // 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
484  // is handled in the algorithm automatically.
485  if(newEdge.Cost >= 0.0 && newEdge.ReverseCost >= 0)
486  {
487  newEdge.Direction = 0;
488  }
489  else if(newEdge.Cost >= 0.0)
490  {
491  newEdge.Direction = 1;
492  }
493  else
494  {
495  newEdge.Direction = -1;
496  }
497 
498  if(edgeIn.id > max_edge_id)
499  {
500  max_edge_id = edgeIn.id;
501  }
502 
503  // Update max_edge_id
504  if(newEdge.StartNode > max_node_id)
505  {
506  return false;//max_node_id = newEdge.StartNode;
507  }
508  if(newEdge.EndNode > max_node_id)
509  {
510  return false;//max_node_id = newEdge.EdgeIndex;
511  }
512 
513  m_vecNodeVector[newEdge.StartNode].xpos = edgeIn.s_x;
514  m_vecNodeVector[newEdge.StartNode].ypos = edgeIn.s_y;
515 
516  m_vecNodeVector[newEdge.EndNode].xpos = edgeIn.t_x;
517  m_vecNodeVector[newEdge.EndNode].ypos = edgeIn.t_y;
518 
519  // update connectivity information for the start node.
520  m_vecNodeVector[newEdge.StartNode].Connected_Nodes.push_back(newEdge.EndNode);
521  m_vecNodeVector[newEdge.StartNode].Connected_Edges_Index.push_back(newEdge.EdgeIndex);
522 
523  // update connectivity information for the end node.
524  m_vecNodeVector[newEdge.EndNode].Connected_Nodes.push_back(newEdge.StartNode);
525  m_vecNodeVector[newEdge.EndNode].Connected_Edges_Index.push_back(newEdge.EdgeIndex);
526 
527 
528 
529  //Adding edge to the list
530  m_mapEdgeId2Index.insert(std::make_pair(newEdge.EdgeID, m_vecEdgeVector.size()));
531  m_vecEdgeVector.push_back(newEdge);
532 
533  //
534  return true;
535 }