stList *chooseMatching_greedy(stList *edges, int64_t nodeNumber) {
/*
* Greedily picks the edge from the list such that each node has at most one edge.
*/
//First clone the list..
edges = stList_copy(edges, NULL);
stSortedSet *seen = getEmptyNodeOrEdgeSetWithCleanup();
stList *matching = stList_construct();
//Sort the adjacency pairs..
stList_sort(edges, chooseMatching_greedyP);
double strength = INT64_MAX;
while (stList_length(edges) > 0) {
stIntTuple *edge = stList_pop(edges);
double d = stIntTuple_get(edge, 2);
assert(d <= strength);
strength = d;
if(!nodeInSet(seen, stIntTuple_get(edge, 0)) && !nodeInSet(seen, stIntTuple_get(edge, 1))) {
addNodeToSet(seen, stIntTuple_get(edge, 0));
addNodeToSet(seen, stIntTuple_get(edge, 1));
stList_append(matching,edge);
}
}
assert(stList_length(edges) == 0);
stList_destruct(edges);
stSortedSet_destruct(seen);
return matching;
}
void AStar::bidirectional(Node _start, Node _end){
Node goal = _end;
Node start = _start;
came_from_map[start.id] = -1;
addNodeToSet(openSet,start);
return;
}
static stSortedSet *getOddNodes(stList *cycle) {
/*
* Returns alternating nodes in a simple cycle.
*/
//Set to return
stSortedSet *nodes = stSortedSet_construct3(
(int(*)(const void *, const void *)) stIntTuple_cmpFn,
(void(*)(void *)) stIntTuple_destruct);
stHash *nodesToEdges = getNodesToEdgesHash(cycle);
int64_t node = stIntTuple_get(stList_get(cycle, 0), 0);
int64_t pNode = -1;
int64_t counter = 0;
bool b = 1;
assert(stList_length(cycle) % 2 == 0);
while (counter++ < stList_length(cycle)) {
if (b) { //Make alternating
addNodeToSet(nodes, node);
b = 0;
} else {
b = 1;
}
stList *edges = getItemForNode(node, nodesToEdges);
assert(stList_length(edges) == 2);
stIntTuple *edge = stList_get(edges, 0);
int64_t node2 = getOtherPosition(edge, node);
if (node2 != pNode) {
pNode = node;
node = node2;
continue;
}
edge = stList_get(edges, 1);
node2 = getOtherPosition(edge, node);
assert(node2 != pNode);
pNode = node;
node = node2;
}
stHash_destruct(nodesToEdges);
assert(stList_length(cycle) / 2 == stSortedSet_size(nodes));
return nodes;
}
void AStar::update(Node current, Node goal){
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
continue;
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
return;
}
bool AStar::getShortestPath(Node _start, Node _end){
Node goal = _end;
Node start =_start;
came_from_map[start.id] = -1;
addNodeToSet(openSet,start);
while(!(openSet.size() == 0)){
#if DEBUG
printf("Size of open list is %d \n", openSet.size());
#endif
Node current = getMinimumNode(openSet);
#if DEBUG
printf("Expanding node:id= %lld\n", current.id);
#endif
if(current.data == goal.data){
cout<<"\nPath found.Reconstructing the path.\n";
reconstructPath(current.id,0);
return true;
}
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf ("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
Node nodeInClosedList = getNodeFromSet(closedSet,nbr);
int temp_g_score = current.g_score + distance(current,nbr);
if(temp_g_score < nodeInClosedList.g_score){
printf(" Removing a Node from closed list id %lld in previous %lld \n", nodeInClosedList.id,came_from_map[nodeInClosedList.id]);
printf(" New gscore is %d , old gscore is %d new wannabe parent %lld\n",temp_g_score, nodeInClosedList.g_score,current.id );
printf(" hscore for %lld wannabe parent %d \n" ,current.id , H(current,goal));
//printf(" hscore for %lld previous parent \n" ,current.id , H(current,goal));
// reconstructPath(nodeInClosedList.id],0);
//exit(0);
removeNodeFromSet(closedSet,nodeInClosedList);
}
else{
continue;
}
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
}
printf("Solution Not Found. Goal is not reachable from start\n");
return false;
}
