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