int findPathA(cell start,cell end,path& best_path,tilemap& t_map, double (*h)(cell x,cell y) ) { int width = 0; int height = 0; int is_ok = 0; cell current; width = t_map[0].size(); height = t_map.size(); costmap G(height,std::vector<double>(width, infinite)); costmap F(height,std::vector<double>(width, infinite)); G[start.second][start.first] = 0; F[start.second][start.first] = G[start.second][start.first] + dist_coeff*h(start,end); std::set<cell > wawe; std::set<cell > closed; std::map<cell,cell > from; std::set<cell > unclosedNeighbours; from[start] = start; wawe.insert(start); while(!wawe.empty()) { current = findMinF(wawe,F); if(current == end){is_ok = 1;break;} wawe.erase(current); closed.insert(current); getUnclosedNeighbours(current,t_map,closed,unclosedNeighbours); for(auto it = unclosedNeighbours.begin();it != unclosedNeighbours.end();it++) { double temp_G = G[current.second][current.first] + transition_cost(from[current],current,*it); if(wawe.count(*it) == 0 || temp_G < G[it->second][it->first]) { from[*it] = current; G[it->second][it->first] = temp_G; F[it->second][it->first] = G[it->second][it->first] + dist_coeff*h(*it,end); wawe.insert(*it); } } } if(!is_ok) return -1; best_path.clear(); best_path.push_back(end); while(current != start) { best_path.push_back(from[current]); current = from[current]; } best_path = std::vector<cell >(best_path.rbegin(),best_path.rend()); return 0; }
void clear() { _self_path.clear(); _target.clear(); _make.clear(); }