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; }