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