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