MapPath Map::pathfind( Vector2d start, Vector2d goal ){
set< pair < float , Vector2d > > open;
Tile& start_tile = (*this)[ start ];
start_tile.position = start;
start_tile.g_score = 0;
start_tile.h_score = h_score( start_tile, goal );
start_tile.f_score = start_tile.h_score;
open.insert( make_pair( (*this)[ start ].f_score, start ) );
size_t icount = 0;
while( !open.empty() ) {
if( icount++ > 100 ) break;
Tile& current = (*this)[ open.begin()->second ];
open.erase( open.begin() );
current.closed = true;
if( current.position == goal ) break;
if( current.status().is_blocked() ) continue;
Vector2d diffs[] = { Vector2d(1,0), Vector2d(-1,0), Vector2d(0,1), Vector2d(0,-1) };
for( int i = 0; i < sizeof(diffs)/sizeof(Vector2d); ++i ) {
Vector2d new_position = current.position + diffs[i];
Tile& new_tile = (*this)[ new_position ];
if( new_tile.closed ) continue;
float new_g = current.g_score + 1;
bool better_g = false;
if( new_tile.g_score < 0 ){
new_tile.h_score = h_score( (*this)[ new_position ], goal );
better_g = true;
} else if( new_g < new_tile.g_score ) {
open.erase( make_pair( new_tile.f_score, new_position ) );
better_g = true;
}
if( better_g ) {
new_tile.g_score = new_g;
new_tile.f_score = new_tile.h_score + new_tile.g_score;
new_tile.origin = current.position;
open.insert( make_pair( new_tile.f_score, new_position ) );
}
}
}
MapPath path;
Vector2d pos = goal;
while( pos != start ) {
path.push_back( pos );
pos = (*this)[ pos ].origin;
}
return path;
}
bool PathFinder::makeMap(const Coord& start, Heuristic heur, EndCondition endCondition, Coord* end)
{
m_openSet.clear();
m_closedSet.clear();
std::priority_queue<DistNode> openQueue;
g_score(start) = 0;
h_score(start) = heur(start);
openQueue.push( DistNode( start, h_score(start) ) );
m_openSet.add(start);
while ( !openQueue.empty() )
{
DistNode dn = openQueue.top();
Coord c = dn.coord;
openQueue.pop();
if (g_score(c) + h_score(c) == dn.f_score)
{
// this entry in the queue is valid
assert(!m_closedSet.has(c));
if (endCondition(c))
{
*end = c;
return true;
}
m_openSet.remove(c);
m_closedSet.add(c);
unsigned int currentGScore = g_score(c);
for (int dir = 0; dir < 8; ++dir)
{
Coord next = c.next(dir);
if (m_closedSet.has(next) || !m_field.isPassable(next))
{
continue;
}
unsigned int newG = currentGScore + m_field.passingTime(c, dir);
bool newNode = false;
if (!m_openSet.has(next))
{
h_score(next) = heur(next);
m_openSet.add(next);
newNode = true;
}
if (newNode || newG < g_score(next))
{
g_score(next) = newG;
openQueue.push(DistNode(next, newG + h_score(next))); // may occur more than once, because we cannot remove the old one
}
}
}
}
return false;
}
