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
BiDirDijkstra.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 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 #if defined(__MINGW32__) || defined(_MSC_VER)
30 #include <winsock2.h>
31 #include <windows.h>
32 #endif
33 
34 #include <queue>
35 #include <vector>
36 #include <functional>
37 
38 #include "BiDirDijkstra.h"
39 #include "../../common/src/pgr_alloc.hpp"
40 
41 
42 
44 }
45 
47 }
48 
50  // max_edge_id = 0;
51  // max_node_id = 0;
52 }
53 
54 /*
55  Initialization and allocation of memories.
56 */
58  int i;
59  m_vecPath.clear();
60  // DBG("BiDirDijkstra::initall: allocating m_pFParent, m_pRParent maxNode: %d\n", maxNode+1);
61  m_pFParent = new PARENT_PATH[maxNode + 1];
62  m_pRParent = new PARENT_PATH[maxNode + 1];
63  // DBG("BiDirDijkstra::initall: allocated m_pFParent, m_pRParent\n");
64 
65  // DBG("BiDirDijkstra::initall: allocating m_pFCost, m_pRCost maxNode: %d\n", maxNode+1);
66  m_pFCost = new double[maxNode + 1];
67  m_pRCost = new double[maxNode + 1];
68  // DBG("BiDirDijkstra::initall: allocated m_pFCost, m_pRCost\n");
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  // DBG("BiDirDijkstra::initall: m_vecNodeVector.reserve(%d)\n", maxNode + 1);
80  // reserve space for nodes
81  m_vecNodeVector.reserve(maxNode + 1);
82  // DBG(" space reserved!\n");
83 }
84 
85 /*
86  Delete the allocated memories to avoid memory leak.
87 */
89  std::vector<GraphNodeInfo*>::iterator itNode;
90  for (itNode = m_vecNodeVector.begin(); itNode != m_vecNodeVector.end(); itNode++) {
91  delete *itNode;
92  }
93  m_vecNodeVector.clear();
94  std::vector<GraphEdgeInfo*>::iterator itEdge;
95  for (itEdge = m_vecEdgeVector.begin(); itEdge != m_vecEdgeVector.end(); itEdge++) {
96  delete *itEdge;
97  }
98  m_vecEdgeVector.clear();
99  delete [] m_pFParent;
100  delete [] m_pRParent;
101  delete [] m_pFCost;
102  delete [] m_pRCost;
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 double BiDirDijkstra::getcost(int node_id, int dir) {
109  if (dir == 1) {
110  return(m_pFCost[node_id]);
111  } else {
112  return(m_pRCost[node_id]);
113  }
114 }
115 /*
116  Set the forward or reverse cost list depending on dir (1 for forward search and -1 for reverse search.
117 */
118 void BiDirDijkstra::setcost(int node_id, int dir, double c) {
119  if (dir == 1) {
120  m_pFCost[node_id] = c;
121  } else {
122  m_pRCost[node_id] = c;
123  }
124 }
125 
126 void BiDirDijkstra::setparent(int node_id, int dir, int parnode, int paredge) {
127  if (dir == 1) {
128  m_pFParent[node_id].par_Node = parnode;
129  m_pFParent[node_id].par_Edge = paredge;
130  } else {
131  m_pRParent[node_id].par_Node = parnode;
132  m_pRParent[node_id].par_Edge = paredge;
133  }
134 }
135 
136 /*
137  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.
138  So one need to recurse up to the source and then add the current node and edge to the list.
139 */
141  if (m_pFParent[node_id].par_Node == -1)
142  return;
143  fconstruct_path(m_pFParent[node_id].par_Node);
144  path_element_t pt;
145  pt.vertex_id = m_pFParent[node_id].par_Node;
146  pt.edge_id = m_pFParent[node_id].par_Edge;
147  pt.cost = m_pFCost[node_id] - m_pFCost[m_pFParent[node_id].par_Node];
148  m_vecPath.push_back(pt);
149 }
150 
151 /*
152  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
153  and edge to the list and then recurse through the parent up to hitting a -1.
154 */
155 
157  path_element_t pt;
158  if (m_pRParent[node_id].par_Node == -1) {
159  pt.vertex_id = node_id;
160  pt.edge_id = -1;
161  pt.cost = 0.0;
162  return;
163  }
164  pt.vertex_id = node_id;
165  pt.cost = m_pRCost[node_id] - m_pRCost[m_pRParent[node_id].par_Node];
166  pt.edge_id = m_pRParent[node_id].par_Edge;
167  m_vecPath.push_back(pt);
168  rconstruct_path(m_pRParent[node_id].par_Node);
169 }
170 
171 /*
172  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
173  que is also passed as parameter que. The current node and the current costs are also available as parameter.
174 */
175 
176 void BiDirDijkstra::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que) {
177  int i;
178  // Number of connected edges
179  int con_edge = static_cast<int>(m_vecNodeVector[cur_node]->Connected_Edges_Index.size());
180  double edge_cost;
181 
182  for (i = 0; i < con_edge; i++) {
183  int edge_index = m_vecNodeVector[cur_node]->Connected_Edges_Index[i];
184  // Get the edge from the edge list.
185  GraphEdgeInfo edge = *m_vecEdgeVector[edge_index];
186  // Get the connected node
187  int new_node = m_vecNodeVector[cur_node]->Connected_Nodes[i];
188 
189  if (cur_node == edge.StartNode) {
190  // Current node is the startnode of the edge. For forward search it should use forward cost, otherwise it should use the reverse cost,
191  // i.e. if the reverse direction is valid then this node may be visited from the end node.
192  if (dir > 0)
193  edge_cost = edge.Cost;
194  else
195  edge_cost = edge.ReverseCost;
196 
197  // Check if the direction is valid for exploration
198  if (edge.Direction == 0 || edge_cost >= 0.0) {
199  // Check if the current edge gives better result
200  if (cur_cost + edge_cost < getcost(new_node, dir)) {
201  // explore the node, and push it in the queue
202  setcost(new_node, dir, cur_cost + edge_cost);
203  setparent(new_node, dir, cur_node, edge.EdgeID);
204  que.push(std::make_pair(cur_cost + edge_cost, new_node));
205 
206  // Update the minimum cost found so far.
207  if (getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost) {
208  m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
209  m_MidNode = new_node;
210  }
211  }
212  }
213  } else {
214  // Current node is the endnode of the edge. For forward search it should use reverse cost, otherwise it should use the forward cost,
215  // i.e. if the forward direction is valid then this node may be visited from the start node.
216  if (dir > 0)
217  edge_cost = edge.ReverseCost;
218  else
219  edge_cost = edge.Cost;
220 
221  // Check if the direction is valid for exploration
222  if (edge.Direction == 0 || edge_cost >= 0.0) {
223  // Check if the current edge gives better result
224  if (cur_cost + edge_cost < getcost(new_node, dir)) {
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, 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  }
237  }
238 }
239 
240 /*
241  This is the entry function that the wrappers should call. Most of the parameters are trivial. maxNode refers to Maximum
242  node id. As we run node based exploration cost, parent etc will be based on maximam node id.
243 */
244 
245 
246 int BiDirDijkstra::bidir_dijkstra(edge_t *edges, unsigned int edge_count, int maxNode, int start_vertex, int end_vertex,
247  path_element_t **path, int *path_count, char **err_msg) {
249  max_edge_id = -1;
250 
251  // Allocate memory for local storage like cost and parent holder
252  // DBG("calling initall(maxNode = %d)\n", maxNode);
253  initall(maxNode);
254 
255  // construct the graph from the edge list, i.e. populate node and edge data structures
256  // DBG("Calling construct_graph\n");
257  construct_graph(edges, edge_count, maxNode);
258 
259 
260  // int nodeCount = m_vecNodeVector.size();
261  // DBG("Setting up std::priority_queue\n");
262  std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > fque;
263  std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > rque;
264 
265  // DBG("calling m_vecPath.clear()\n");
266  m_vecPath.clear();
267 
268  // Initialize the forward search
269  m_pFParent[start_vertex].par_Node = -1;
270  m_pFParent[start_vertex].par_Edge = -1;
271  m_pFCost[start_vertex] = 0.0;
272  fque.push(std::make_pair(0.0, start_vertex));
273 
274  // Initialize the reverse search
275  m_pRParent[end_vertex].par_Node = -1;
276  m_pRParent[end_vertex].par_Edge = -1;
277  m_pRCost[end_vertex] = 0.0;
278  rque.push(std::make_pair(0.0, end_vertex));
279 
280  int i;
281  // int new_node;
282  int cur_node;
283  // int dir;
284 
285 /*
286  The main loop. The algorithm is as follows:
287  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.
288  2. IF the reverse heap minimum is lower than the forward heap minimum, explore from reverse direction.
289  3. ELSE explore from the forward directtion.
290 */
291 
292  while (!fque.empty() && !rque.empty()) {
293  PDI fTop = fque.top();
294  PDI rTop = rque.top();
295  if (fTop.first + rTop.first > m_MinCost) // We are done, there is no path with lower cost
296  break;
297 
298  if (rTop.first < fTop.first) {
299  // Explore from reverse queue
300  cur_node = rTop.second;
301  int dir = -1;
302  rque.pop();
303  explore(cur_node, rTop.first, dir, rque);
304  } else {
305  // Explore from forward queue
306  cur_node = fTop.second;
307  int dir = 1;
308  fque.pop();
309  explore(cur_node, fTop.first, dir, fque);
310  }
311  }
312 
313 /*
314  Path reconstruction part. m_MidNode is the joining point where two searches meet to make a shortest path. It is updated in explore.
315  If it contains -1, then no path is found. Other wise we have a shortest path and that is reconstructed in the m_vecPath.
316 */
317  if (m_MidNode == -1) {
318  *err_msg = (char *)"Path Not Found";
319  deleteall();
320  return -1;
321  } else {
322  // reconstruct path from forward search
324  // reconstruct path from backward search
326 
327  // insert the last row in the path trace (having edge_id = -1 and cost = 0.0)
328  path_element_t pelement;
329  pelement.vertex_id = end_vertex;
330  pelement.edge_id = -1;
331  pelement.cost = 0.0;
332  m_vecPath.push_back(pelement);
333 
334  // Transfer data path to path_element_t format and allocate memory and populate the pointer
335 
336  // DBG("BiDirDijkstra::bidir_dijkstra: allocating path m_vecPath.size = %d\n", m_vecPath.size() + 1);
337  // *path = (path_element_t *) malloc(sizeof(path_element_t) * (m_vecPath.size() + 1));
338  *path = NULL;
339  *path = pgr_alloc(m_vecPath.size(), (*path));
340  *path_count = static_cast<int>(m_vecPath.size());
341  // DBG("BiDirDijkstra::bidir_dijkstra: allocated path\n");
342 
343  for (i = 0; i < *path_count; i++) {
344  (*path)[i].vertex_id = m_vecPath[i].vertex_id;
345  (*path)[i].edge_id = m_vecPath[i].edge_id;
346  (*path)[i].cost = m_vecPath[i].cost;
347  }
348  }
349  // DBG("calling deleteall\n");
350  deleteall();
351  // DBG("back from deleteall\n");
352  return 0;
353 }
354 
355 /*
356  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
357  corresponding edge indices from edge list that connect this node with the adjacent nodes.
358 */
359 
361  int i;
362 
363  /*
364  // Create a dummy node
365  DBG("Create a dummy node\n");
366  GraphNodeInfo nodeInfo;
367  DBG("calling nodeInfo.Connected_Edges_Index.clear\n");
368  nodeInfo.Connected_Edges_Index.clear();
369  DBG("calling nodeInfo.Connected_Nodes.clear\n");
370  nodeInfo.Connected_Nodes.clear();
371  */
372 
373  // 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.
374  // 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.
375  // DBG("m_vecNodeVector.push_back for 0 - %d\n", maxNode);
376  for (i = 0; i <= maxNode; i++) {
377  // Create a dummy node
378  GraphNodeInfo* nodeInfo = new GraphNodeInfo();
379  nodeInfo->Connected_Edges_Index.clear();
380  nodeInfo->Connected_Nodes.clear();
381 
382  nodeInfo->NodeID = i;
383  m_vecNodeVector.push_back(nodeInfo);
384  }
385 
386  // Process each edge from the edge list and update the member data structures accordingly.
387  // DBG("reserving space for m_vecEdgeVector.reserve(%d)\n", edge_count);
388  m_vecEdgeVector.reserve(edge_count);
389  // DBG("calling addEdge in a loop\n");
390  for (i = 0; i < edge_count; i++) {
391  addEdge(edges[i]);
392  }
393 
394  return true;
395 }
396 
397 /*
398  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
399  connectivity information needs to be updated.
400 */
401 
403  // long lTest;
404 
405  // Check if the edge is already processed.
406  Long2LongMap::iterator itMap = m_mapEdgeId2Index.find(edgeIn.id);
407  if (itMap != m_mapEdgeId2Index.end())
408  return false;
409 
410 
411  // Create a GraphEdgeInfo using the information of the current edge
412  GraphEdgeInfo *newEdge = new GraphEdgeInfo();
413  newEdge->EdgeID = static_cast<int>(edgeIn.id);
414  newEdge->EdgeIndex = static_cast<int>(m_vecEdgeVector.size());
415  newEdge->StartNode = static_cast<int>(edgeIn.source);
416  newEdge->EndNode = static_cast<int>(edgeIn.target);
417  newEdge->Cost = edgeIn.cost;
418  newEdge->ReverseCost = edgeIn.reverse_cost;
419 
420  // 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
421  // negative then the edge is unidirectional with direction = 1 (goes from source to target) otherwise it is unidirectional with direction = -1 (goes from target
422  // 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
423  // is handled in the algorithm automatically.
424  if (newEdge->Cost >= 0.0 && newEdge->ReverseCost >= 0) {
425  newEdge->Direction = 0;
426  } else if (newEdge->Cost >= 0.0) {
427  newEdge->Direction = 1;
428  } else {
429  newEdge->Direction = -1;
430  }
431 
432  // Update max_edge_id
433  if (edgeIn.id > max_edge_id) {
434  max_edge_id = static_cast<int>(edgeIn.id);
435  }
436 
437  // Update max_node_id
438  if (newEdge->StartNode > max_node_id) {
439  return false; // max_node_id = newEdge.StartNode;
440  }
441  if (newEdge->EndNode > max_node_id) {
442  return false; // max_node_id = newEdge.EdgeIndex;
443  }
444 
445  // update connectivity information for the start node.
446  m_vecNodeVector[newEdge->StartNode]->Connected_Nodes.push_back(newEdge->EndNode);
447  m_vecNodeVector[newEdge->StartNode]->Connected_Edges_Index.push_back(newEdge->EdgeIndex);
448 
449  // update connectivity information for the start node.
450  m_vecNodeVector[newEdge->EndNode]->Connected_Nodes.push_back(newEdge->StartNode);
451  m_vecNodeVector[newEdge->EndNode]->Connected_Edges_Index.push_back(newEdge->EdgeIndex);
452 
453 
454 
455  // Adding edge to the list
456  m_mapEdgeId2Index.insert(std::make_pair(newEdge->EdgeID, m_vecEdgeVector.size()));
457  m_vecEdgeVector.push_back(newEdge);
458 
459  return true;
460 }
bool addEdge(edge_t edgeIn)
double reverse_cost
Definition: pgr_types.h:132
int path_count
Definition: BDATester.cpp:51
double * m_pFCost
int64_t source
Definition: pgr_types.h:129
std::vector< size_t > Connected_Edges_Index
int64_t target
Definition: pgr_types.h:130
GraphEdgeVector m_vecEdgeVector
double getcost(int node_id, int dir)
std::pair< double, int > PDI
#define INF
double ReverseCost
std::vector< int > Connected_Nodes
PARENT_PATH * m_pRParent
void fconstruct_path(int node_id)
void rconstruct_path(int node_id)
bool construct_graph(edge_t *edges, int edge_count, int maxNode)
GraphNodeVector m_vecNodeVector
Long2LongMap m_mapEdgeId2Index
int64_t id
Definition: pgr_types.h:128
int maxNode
Definition: BDATester.cpp:47
double cost
Definition: pgr_types.h:131
int edge_count
Definition: BDATester.cpp:47
edge_astar_t * edges
Definition: BDATester.cpp:46
void explore(int cur_node, double cur_cost, int dir, std::priority_queue< PDI, std::vector< PDI >, std::greater< PDI > > &que)
void setcost(int node_id, int dir, double c)
void setparent(int node_id, int dir, int parnode, int paredge)
int64_t edge_id
Definition: pgr_types.h:75
T * pgr_alloc(std::size_t size, T *ptr)
allocates memory
Definition: pgr_alloc.hpp:52
int bidir_dijkstra(edge_t *edges, unsigned int edge_count, int maxNode, int start_vertex, int end_vertex, path_element_t **path, int *path_count, char **err_msg)
path_element_t * path
Definition: BDATester.cpp:49
int64_t vertex_id
Definition: pgr_types.h:74
char * err_msg
Definition: BDATester.cpp:50
double * m_pRCost
std::vector< path_element_t > m_vecPath
void initall(int maxNode)
PARENT_PATH * m_pFParent
double cost
Definition: pgr_types.h:76