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