Beispiel #1
0
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;
}
Beispiel #2
0
 void clear()
 {
  _self_path.clear();
  _target.clear();
  _make.clear();
 }