void BiDirAStar::fconstruct_path(int node_id) {
if (m_pFParent[node_id].par_Node == -1)
return;
fconstruct_path(m_pFParent[node_id].par_Node);
path_element_t pt;
pt.vertex_id = m_pFParent[node_id].par_Node;
pt.edge_id = m_pFParent[node_id].par_Edge;
pt.cost = m_pFCost[node_id] - m_pFCost[m_pFParent[node_id].par_Node];
m_vecPath.push_back(pt);
}
int BiDirAStar:: 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)
{
max_node_id = maxNode;
max_edge_id = -1;
// Allocate memory for local storage like cost and parent holder
initall(maxNode);
// construct the graph from the edge list, i.e. populate node and edge data structures
construct_graph(edges, edge_count, maxNode);
m_lStartNodeId = start_vertex;
m_lEndNodeId = end_vertex;
// int nodeCount = m_vecNodeVector.size();
MinHeap fque(maxNode + 2);
MinHeap rque(maxNode + 2);
//std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > fque;
//std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > rque;
m_vecPath.clear();
// Initialize the forward search
m_pFParent[start_vertex].par_Node = -1;
m_pFParent[start_vertex].par_Edge = -1;
m_pFCost[start_vertex] = 0.0;
fque.push(std::make_pair(0.0, start_vertex));
// Initialize the reverse search
m_pRParent[end_vertex].par_Node = -1;
m_pRParent[end_vertex].par_Edge = -1;
m_pRCost[end_vertex] = 0.0;
rque.push(std::make_pair(0.0, end_vertex));
// int new_node;
int cur_node;
// int dir;
/*
The main loop. The algorithm is as follows:
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.
2. IF the reverse heap minimum is lower than the forward heap minimum, explore from reverse direction.
3. ELSE explore from the forward directtion.
*/
while(!fque.empty() && !rque.empty())
{
PDI fTop = fque.top();
PDI rTop = rque.top();
if(m_pFCost[fTop.second] + m_pRCost[rTop.second] > m_MinCost) //We are done, there is no path with lower cost
break;
if(rTop.first < fTop.first) // Explore from reverse queue
{
if(rTop.first > m_MinCost)
break;
cur_node = rTop.second;
int dir = -1;
rque.pop();
explore(cur_node, m_pRCost[rTop.second], dir, rque);
}
else // Explore from forward queue
{
if(fTop.first > m_MinCost)
break;
cur_node = fTop.second;
int dir = 1;
fque.pop();
explore(cur_node, m_pFCost[fTop.second], dir, fque);
}
}
/*
Path reconstruction part. m_MidNode is the joining point where two searches meet to make a shortest path. It is updated in explore.
If it contains -1, then no path is found. Other wise we have a shortest path and that is reconstructed in the m_vecPath.
*/
if(m_MidNode == -1)
{
*err_msg = (char *)"Path Not Found";
deleteall();
return -1;
}
else
{
// reconstruct path from forward search
fconstruct_path(m_MidNode);
// reconstruct path from backward search
rconstruct_path(m_MidNode);
// insert the last row in the path trace (having edge_id = -1 and cost = 0.0)
path_element_t pelement;
pelement.vertex_id = end_vertex;
pelement.edge_id = -1;
pelement.cost = 0.0;
m_vecPath.push_back(pelement);
// Transfer data path to path_element_t format and allocate memory and populate the pointer
*path = (path_element_t *) malloc(sizeof(path_element_t) * (m_vecPath.size() + 1));
*path_count = m_vecPath.size();
for(size_t i = 0; i < *path_count; i++)
{
(*path)[i].vertex_id = m_vecPath[i].vertex_id;
(*path)[i].edge_id = m_vecPath[i].edge_id;
(*path)[i].cost = m_vecPath[i].cost;
}
}
deleteall();
return 0;
}
int BiDirDijkstra:: 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)
{
max_node_id = maxNode;
max_edge_id = -1;
init();
construct_graph(edges, edge_count, maxNode);
int nodeCount = m_vecNodeVector.size();
std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > fque;
std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > rque;
m_vecPath.clear();
int i;
initall(maxNode);
m_pFParent[start_vertex].par_Node = -1;
m_pFParent[start_vertex].par_Edge = -1;
m_pFCost[start_vertex] = 0.0;
fque.push(std::make_pair(0.0, start_vertex));
m_pRParent[end_vertex].par_Node = -1;
m_pRParent[end_vertex].par_Edge = -1;
m_pRCost[end_vertex] = 0.0;
rque.push(std::make_pair(0.0, end_vertex));
int new_node;
int cur_node;
int dir;
while(!fque.empty() && !rque.empty())
{
PDI fTop = fque.top();
PDI rTop = rque.top();
if(fTop.first + rTop.first > m_MinCost) //We are done, there is no path with lower cost
break;
if(rTop.first < fTop.first) // Explore from reverse queue
{
cur_node = rTop.second;
int dir = -1;
rque.pop();
explore(cur_node, rTop.first, dir, rque);
}
else // Explore from forward queue
{
cur_node = fTop.second;
int dir = 1;
fque.pop();
explore(cur_node, fTop.first, dir, fque);
}
}
if(m_MidNode == -1)
{
*err_msg = (char *)"Path Not Found";
deleteall();
return -1;
}
else
{
fconstruct_path(m_MidNode);
rconstruct_path(m_MidNode);
path_element_t pelement;
pelement.vertex_id = end_vertex;
pelement.edge_id = -1;
pelement.cost = 0.0;
m_vecPath.push_back(pelement);
*path = (path_element_t *) malloc(sizeof(path_element_t) * (m_vecPath.size() + 1));
*path_count = m_vecPath.size();
for(i = 0; i < *path_count; i++)
{
(*path)[i].vertex_id = m_vecPath[i].vertex_id;
(*path)[i].edge_id = m_vecPath[i].edge_id;
(*path)[i].cost = m_vecPath[i].cost;
}
}
deleteall();
return 0;
}