// Find the path between (sx,sy) and (dx,dy). Returns PATH_FAILED or PATH_OK. int PATHFINDER::find_path(int sx_, int sy_, int dx_, int dy_) { // Check that the target tile is reachable if(map_solid(dx_, dy_)) return PATH_FAILED; // Helper array which tells us if a tile is on the open nodes list, or // on the closed nodes list. // 0 == It isn't on the lists // 1 == It is on the closed list // 2 == It is on the open list int map_nodes[MAP_W][MAP_H]; // Initialize the pathfinder dx = dx_; dy = dy_; sx = sx_; sy = sy_; nodes_open.clear(); nodes_closed.clear(); path.clear(); for(int fyy = 0; fyy < MAP_H; fyy++) for(int fxx = 0; fxx < MAP_W; fxx++) map_nodes[fxx][fyy] = 0; // Add the starting point to the open nodes NODE st; st.clear(); st.x = sx; st.y = sy; nodes_open.push_back(st); map_nodes[sx][sy] = 2; // Current node NODE current; current.clear(); // State of the search int search_state = -1; // Search the path while(search_state == -1) { // Sort the open list by F value nodes_open.sort(); // Current tile shall be the one with the lowest F (the first after sorting) current = *(nodes_open.begin()); // Move it to the closed list nodes_open.pop_front(); nodes_closed.push_back(current); map_nodes[current.x][current.y] = 1; // Look for adjacent reachable tiles for(int f=0; f < 4; f++) { int mx,my; switch(f) { default: case 0: mx = current.x; my = current.y - 1; break; case 1: mx = current.x + 1; my = current.y; break; case 2: mx = current.x; my = current.y + 1; break; case 3: mx = current.x - 1; my = current.y; break; } // Check the tile if(map_solid(mx, my)) continue; // Solid tile, skip it // Check if the tile is on the closed nodes list if(map_nodes[mx][my] == 1) continue; /* bool is_closed = false; list<NODE>::iterator i; for(i = nodes_open.begin(); i != nodes_open.end(); ++i) { if((*i).x == mx && (*i).y == my) { is_closed = true; break; } } if(is_closed) continue; // It was on the closed nodes list, skip it */ // Check if the tile isn't on the open nodes list if(map_nodes[mx][my] == 0) { // Add a new node and make the current tile it's parent NODE n; n.clear(); n.x = mx; n.y = my; n.g = current.g + 10; // No diagonal movement n.h = tile_dist(mx, my, dx, dy); // Manhattan distance n.f = n.g + n.h; // F = G + H n.parent = last_node(nodes_closed); nodes_open.push_back(n); map_nodes[mx][my] = 2; // If we just added our target tile (dx, dy), we've found the path. // Bail out. if(mx == dx && my == dy) { search_state = PATH_OK; break; } } } // If the list of open nodes is empty, we can't find the path if(nodes_open.empty()) { search_state = PATH_FAILED; break; } } // At this point, the path is either found or failed if(search_state == PATH_OK) { // Path was found // The destination node is the last node added to the open list NODE *dest = last_node(nodes_open); // Make sure it is so if(dest->x != dx || dest->y != dy) printf("find_path() error:\nThe path destination is (%d,%d) while it should be (%d,%d)!\n", dest->x, dest->y, dx, dy); // Add the destination to the path PATHPOINT point; point.x = dx; point.y = dy; path.insert(path.begin(), point); // Traverse from the destination along the path, and save the path points NODE *p = dest->parent; while(p) { PATHPOINT point; point.x = p->x; point.y = p->y; path.insert(path.begin(), point); p = p->parent; } // Remove the start point from the path as it isn't needed. path.erase(path.begin()); // Clear the nodes list as we don't need them any more nodes_open.clear(); nodes_closed.clear(); return PATH_OK; } else { // Path was not found return PATH_FAILED; } }