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