Trail *Path::searchPath(void *o,int x1,int y1,int x2,int y2,int l) { int i,d,x,y,c; PathNode *p1 = 0,*p2 = 0; debug_output("Path::searchPath(x1=%d,y1=%d,x2=%d,y2=%d)\n",x1,y1,x2,y2); clear(); obj = o,sx = x1,sy = y1,dx = x2,dy = y2; if(sx==dx && sy==dy) return 0; debug_output("Path::searchPath(x1=%d,y1=%d,x2=%d,y2=%d)\n",x1,y1,x2,y2); cap = heur(*this,sx,sy)*8+1; p1 = new PathNode(sx,sy,0,heur(*this,sx,sy),0); closest = p1; put(p1); push(p1); debug_output("Path::searchPath(x1=%d,y1=%d,x2=%d,y2=%d)\n",x1,y1,x2,y2); while(open) { if(step) step(*this,*open); p1 = pop(); debug_output("key=%08x\tx1=%d\ty1=%d\tx2=%d\ty2=%d\ts=%d\tg=%d\th=%d\n",p1->key,p1->x,p1->y, p1->parent? p1->parent->x : -1,p1->parent? p1->parent->y : -1,p1->s,p1->g,p1->h); if(!l || p1->s<l) for(i=0,d=0; d!=-1; ++i) { d = move(*this,*p1,x,y,i); c = weight(*this,*p1,x,y); if(x==dx && y==dy) { if(c!=PATH_CANNOT_MOVE) { p1 = new PathNode(x,y,p1->g+1,0,p1); closest = p1; put(p1); } goto finished; } else if(c!=PATH_CANNOT_MOVE && c!=PATH_AVOID_MOVE) { if((p2=get(x,y))) { if(p1->g+c<p2->g) { remove(p2); p2->parent = p1,p2->g = p1->g+c,p2->s = p1->s+1; push(p2); } } else { p2 = new PathNode(x,y,p1->g+c,heur(*this,x,y),p1); if(p2->h<closest->h || (p2->h==closest->h && p2->g<closest->g)) closest = p2; put(p2); push(p2); } } } } finished: return getTrail(closest); }
void dfs(int board[], int pre, int dp, int maxdp) { int f = heur(board); int tmp[MAXM], i; if(f + dp > maxdp || found) return; if(f == 0) { ops[top + 1] = '\0'; printf("%s\n", ops); printf("%d\n", board[12]); found = 1; return; } for(i = 0;i < 8; i++) { if((pre == 0 && i == 5) || (pre == 5 && i == 0)) continue; if((pre == 1 && i == 4) || (pre == 4 && i == 1)) continue; if((pre == 2 && i == 7) || (pre == 7 && i == 2)) continue; if((pre == 3 && i == 6) || (pre == 6 && i == 3)) continue; memcpy(tmp, board, sizeof(tmp)); mv(tmp, i); ops[++top] = 'A' + i; dfs(tmp, i, dp + 1, maxdp); top--; } }
void ida_star() { int maxdp = heur(board); if(maxdp == 0) { printf("No moves needed\n"); printf("%d\n", board[12]); return; } found = 0; while(found == 0) { top = -1; dfs(board, -1, 0, maxdp); maxdp++; } }
std::vector<node> find_closest(int x_i, int y_i, std::vector<signed char> matrix_array, int val_to_find){ int lateral_size=std::sqrt(matrix_array.size()); std::vector<std::vector<signed char> >matrix(lateral_size); int goal_coords[2]={NO_VAL,NO_VAL},coords[2],from[2],cost=0; std::vector<node> return_error; double t_f=0, t_g=0, t_h=0; // Conversion to matrix for(int i=0; i < lateral_size; i++){ matrix[i].resize(lateral_size,blue); } for(int x=0; x < lateral_size; x++){ for(int y=0; y < lateral_size; y++){ matrix[x][y]=matrix_array[x+y*lateral_size]; } } if(x_i < 0 || y_i < 0 || x_i >= lateral_size || y_i >= lateral_size){ ROS_ERROR("Invalid initial coordinates"); return return_error; } // values different from white and val_to_find are obstacles if(val_to_find==-1){ // back to start ROS_WARN("Will plan a path back to the start"); goal_coords[0]=lateral_size/2; goal_coords[1]=lateral_size/2; ROS_WARN("My goal is set to the start position: (%d,%d)",goal_coords[0],goal_coords[1]); }else{ for(int x=0; x < lateral_size; x++){ for(int y=0; y < lateral_size; y++){ if(matrix[x][y]!=white && matrix[x][y]!=val_to_find){ matrix[x][y]=black; }else if(val_to_find!=gray && matrix[x][y]==val_to_find){ goal_coords[0]=x; goal_coords[1]=y; } } } } if(val_to_find==gray){ // inefficient, but better than running A* without an heuristic for(int i=0; i < lateral_size/2-1 && goal_coords[0]==NO_VAL;i++){ for(int x=x_i-i/2; x < x_i+i/2;x++){ for(int y=y_i-i/2; y<y_i+i/2;y++){ if(matrix[x][y]==gray){ goal_coords[0]=x; goal_coords[1]=y; } } } } } ROS_WARN("My goal is set to: (%d,%d)",goal_coords[0],goal_coords[1]); // A* node current,n; coords[0]=x_i; coords[1]=y_i; from[0]=NO_VAL; from[1]=NO_VAL; t_g=0; t_h=heur(coords,goal_coords); t_f = t_g + t_h; current=create_node(coords,t_f,t_g,from); return_error.push_back(current); if(goal_coords[0]==NO_VAL){ ROS_INFO("No solution exists"); return return_error; } search_set closedset; search_set openset(current); search_set came_from; search_set nodes_set(current); int i=0; signal(SIGINT, &trap); while(!openset.isempty() && execute){ i++; //ROS_INFO("iter: %d",i); current=openset.pop_best(); /*ROS_INFO("Will evaluate node (%d,%d).",current.coords[0],current.coords[1]); ROS_INFO(" t_f = %.2f, t_g = %.2f",current.t_f,current.t_g); getchar();*/ if(current.coords[0]==goal_coords[0] && current.coords[1]==goal_coords[1]){ ROS_INFO("Found solution at %d %d!",current.coords[0],current.coords[1]); return retrieve_path(current, &closedset); } closedset.push_node(current); // FOR EACH NEIGHBOUR if(current.coords[0]-1>=0){ coords[0]=current.coords[0]-1; coords[1]=current.coords[1]; if(matrix[coords[0]][coords[1]] == white || matrix[coords[0]][coords[1]] == val_to_find){ t_h=heur(n.coords,goal_coords); t_g = current.t_g + g_cost(coords,current.coords,current.came_from); t_f = t_g + t_h; if(openset.check_if_in_set(coords)){ n = openset.read_node(coords); // only doing this to get the f value, if present }else{ n = create_node(coords,t_f,t_g,current.coords); } if(!closedset.check_if_in_set(n.coords) || t_f < n.t_f ){ n.t_g=t_g; n.t_f=t_f; n.came_from[0]=current.coords[0]; n.came_from[1]=current.coords[1]; if(!openset.check_if_in_set(n.coords)) openset.push_node(n); else{ // update the value of the node in openset openset.pop_requested(n.coords); openset.push_node(n); } } } } if(current.coords[0]+1<lateral_size){ coords[0]=current.coords[0]+1; coords[1]=current.coords[1]; if(matrix[coords[0]][coords[1]] == white || matrix[coords[0]][coords[1]] == val_to_find){ t_h=heur(n.coords,goal_coords); t_g = current.t_g + g_cost(coords,current.coords,current.came_from); t_f = t_g + t_h; if(openset.check_if_in_set(coords)){ n = openset.read_node(coords); }else{ n = create_node(coords,t_f,t_g,current.coords); } if(!closedset.check_if_in_set(n.coords) || t_f < n.t_f ){ n.t_g=t_g; n.t_f=t_f; n.came_from[0]=current.coords[0]; n.came_from[1]=current.coords[1]; if(!openset.check_if_in_set(n.coords)) openset.push_node(n); else{ openset.pop_requested(n.coords); openset.push_node(n); } } } } if(current.coords[1]-1>=0){ coords[0]=current.coords[0]; coords[1]=current.coords[1]-1; if(matrix[coords[0]][coords[1]] == white || matrix[coords[0]][coords[1]] == val_to_find){ t_h=heur(n.coords,goal_coords); t_g = current.t_g + g_cost(coords,current.coords,current.came_from); t_f = t_g + t_h; if(openset.check_if_in_set(coords)){ n = openset.read_node(coords); }else{ n = create_node(coords,t_f,t_g,current.coords); } if(!closedset.check_if_in_set(n.coords) || t_f < n.t_f ){ n.t_g=t_g; n.t_f=t_f; n.came_from[0]=current.coords[0]; n.came_from[1]=current.coords[1]; if(!openset.check_if_in_set(n.coords)) openset.push_node(n); else{ openset.pop_requested(n.coords); openset.push_node(n); } } } } if(current.coords[1]+1<lateral_size){ coords[0]=current.coords[0]; coords[1]=current.coords[1]+1; if(matrix[coords[0]][coords[1]] == white || matrix[coords[0]][coords[1]] == val_to_find){ t_h=heur(n.coords,goal_coords); t_g = current.t_g + g_cost(coords,current.coords,current.came_from); t_f = t_g + t_h; if(openset.check_if_in_set(coords)){ n = openset.read_node(coords); }else{ n = create_node(coords,t_f,t_g,current.coords); } if(!closedset.check_if_in_set(n.coords) || t_f < n.t_f ){ n.t_g=t_g; n.t_f=t_f; n.came_from[0]=current.coords[0]; n.came_from[1]=current.coords[1]; if(!openset.check_if_in_set(n.coords)) openset.push_node(n); else{ openset.pop_requested(n.coords); openset.push_node(n); } } } } } ROS_INFO("Did not find anything :("); return return_error; }