void link(const value_type& x, const value_type& y) {
if (x == y) return;
rank_type rx = getrank(x), ry = getrank(y);
if (rx < ry)
setparent(x, y);
else
setparent(y, x);
if (rx == ry)
setrank(x, getrank(x) + 1);
}
Wstatus::Wstatus(string name, Wform * parent){
this->name = name;
HWND hWnd = parent->gethandle();
hSW = CreateStatusWindow(WS_CHILD | WS_VISIBLE, NULL, hWnd, wId);
setparent(parent);
}
//void BiDirAStar::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que)
void BiDirAStar::explore(int cur_node, double cur_cost, int dir, MinHeap &que)
{
size_t i;
// Number of connected edges
auto con_edge = m_vecNodeVector[cur_node].Connected_Edges_Index.size();
double edge_cost;
for(i = 0; i < con_edge; i++)
{
auto edge_index = m_vecNodeVector[cur_node].Connected_Edges_Index[i];
// Get the edge from the edge list.
GraphEdgeInfo edge = m_vecEdgeVector[edge_index];
// Get the connected node
int new_node = m_vecNodeVector[cur_node].Connected_Nodes[i];
#if 0 // mult is set but not used
int mult;
if(edge.Direction == 0)
mult = 1;
else
mult = dir;
#endif
if(cur_node == edge.StartNode)
{
// Current node is the startnode of the edge. For forward search it should use forward cost, otherwise it should use the reverse cost,
// i.e. if the reverse direction is valid then this node may be visited from the end node.
if(dir > 0)
edge_cost = edge.Cost;
else
edge_cost = edge.ReverseCost;
// Check if the direction is valid for exploration
if(edge.Direction == 0 || edge_cost >= 0.0)
{
//edge_cost = edge.Cost * mult;
// Check if the current edge gives better result
if(cur_cost + edge_cost < getcost(new_node, dir))
{
// explore the node, and push it in the queue. the value in the queue will also contain the heuristic cost
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost + gethcost(new_node, dir), new_node));
// Update the minimum cost found so far.
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
else
{
// Current node is the endnode of the edge. For forward search it should use reverse cost, otherwise it should use the forward cost,
// i.e. if the forward direction is valid then this node may be visited from the start node.
if(dir > 0)
edge_cost = edge.ReverseCost;
else
edge_cost = edge.Cost;
// Check if the direction is valid for exploration
if(edge.Direction == 0 || edge_cost >= 0.0)
{
//edge_cost = edge.ReverseCost * mult;
// Check if the current edge gives better result
if(cur_cost + edge_cost < getcost(new_node, dir))
{
// explore the node, and push it in the queue. the value in the queue will also contain the heuristic cost
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost + gethcost(new_node, dir), new_node));
// Update the minimum cost found so far.
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
}
}
void BiDirDijkstra::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que)
{
int i;
// Number of connected edges
int con_edge = m_vecNodeVector[cur_node]->Connected_Edges_Index.size();
double edge_cost;
for(i = 0; i < con_edge; i++)
{
int edge_index = m_vecNodeVector[cur_node]->Connected_Edges_Index[i];
// Get the edge from the edge list.
GraphEdgeInfo edge = m_vecEdgeVector[edge_index];
// Get the connected node
int new_node = m_vecNodeVector[cur_node]->Connected_Nodes[i];
if(cur_node == edge.StartNode)
{
// Current node is the startnode of the edge. For forward search it should use forward cost, otherwise it should use the reverse cost,
// i.e. if the reverse direction is valid then this node may be visited from the end node.
if(dir > 0)
edge_cost = edge.Cost;
else
edge_cost = edge.ReverseCost;
// Check if the direction is valid for exploration
if(edge.Direction == 0 || edge_cost >= 0.0)
{
// Check if the current edge gives better result
if(cur_cost + edge_cost < getcost(new_node, dir))
{
// explore the node, and push it in the queue
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost, new_node));
// Update the minimum cost found so far.
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
else
{
// Current node is the endnode of the edge. For forward search it should use reverse cost, otherwise it should use the forward cost,
// i.e. if the forward direction is valid then this node may be visited from the start node.
if(dir > 0)
edge_cost = edge.ReverseCost;
else
edge_cost = edge.Cost;
// Check if the direction is valid for exploration
if(edge.Direction == 0 || edge_cost >= 0.0)
{
// Check if the current edge gives better result
if(cur_cost + edge_cost < getcost(new_node, dir))
{
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost, new_node));
// Update the minimum cost found so far.
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
}
}
void BiDirDijkstra::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que)
{
int i;
int con_edge = m_vecNodeVector[cur_node].Connected_Edges_Index.size();
double edge_cost;
for(i = 0; i < con_edge; i++)
{
int edge_index = m_vecNodeVector[cur_node].Connected_Edges_Index[i];
GraphEdgeInfo edge = m_vecEdgeVector[edge_index];
int new_node = m_vecNodeVector[cur_node].Connected_Nodes[i];
int mult;
if(edge.Direction == 0)
mult = 1;
else
mult = dir;
if(cur_node == edge.StartNode)
{
if(dir > 0)
edge_cost = edge.Cost;
else
edge_cost = edge.ReverseCost;
if(edge.Direction == 0 || edge_cost >= 0.0)
{
//edge_cost = edge.Cost * mult;
if(cur_cost + edge_cost < getcost(new_node, dir))
{
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost, new_node));
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
else
{
if(dir > 0)
edge_cost = edge.ReverseCost;
else
edge_cost = edge.Cost;
if(edge.Direction == 0 || edge_cost >= 0.0)
{
//edge_cost = edge.ReverseCost * mult;
if(cur_cost + edge_cost < getcost(new_node, dir))
{
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost, new_node));
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
}
}
value_type path_compression(const value_type& x) {
if (getparent(x) != x)
setparent(x, path_compression(getparent(x)));
return getparent(x);
}
void make_set(const value_type& x) {
setparent(x, x);
setrank(x, 1);
}