Whoa!
I've been working on the A* pathfinder for GrogBot 2.0 lately. I never thought that I could be able to understand such a big thing as A* is, but when I finally started gathering information about it and writing my own implementation, I began to understand how A* actually works.
I got it working pretty fast, but it was really SLOW at first. Unusable... It took half a minute to calculate a path in a map with ~200 nodes. After some little optimizations, I got it to calculate the path in 1-2 seconds. More faster than before, but still far too slow.
It took me two days to understand what the problem was :|. I was logging every GetItemCount() call to an external file, which slowed it down A LOT. After removing those two little lines of code, it's working a lot better. Sure it isn't the most optimized one, but it works!
void CPathfinder::FindPath( CNode *start, CNode *end )
{
m_bSearching = true;
SAFE_DELETE( m_pResult );
SAFE_DELETE( m_pOpenList );
m_pOpenList = new CList<astar_t>;
// Store the indexes of the start and end nodes
m_iStartNodeIndex = g_NodeManager.GetNodeIndex( start );
m_iEndNodeIndex = g_NodeManager.GetNodeIndex( end );
for ( int i = 0; i < g_NodeManager.GetNodeCount(); i++ )
{
// Initialize the values.
m_Nodes[i].open = false;
m_Nodes[i].closed = false;
m_Nodes[i].f = 0.0;
m_Nodes[i].cost = 0.0;
m_Nodes[i].heuristic = 0.0;
m_Nodes[i].parent = -1;
m_Nodes[i].index = i;
m_Nodes[i].pNode = g_NodeManager.GetNodeByIndex( i );
}
// Mark the start node as open
m_Nodes[m_iStartNodeIndex].open = true;
m_pOpenList->Insert( &m_Nodes[m_iStartNodeIndex] );
Advance();
}
void CPathfinder::Advance( )
{
int i;
if (!m_bSearching)
return;
int loops = 0;
while ((m_bSearching) && (loops < 100))
{
loops++;
float smallest_f = -1.0;
astar_t *pChosen = NULL;
// Find the node with the smallest f from the open list.
for ( i = 0; i < m_pOpenList->GetItemCount(); i++ )
{
astar_t *pCurrent = m_pOpenList->Get(i);
if (!pCurrent)
continue;
if ( (pCurrent->f < smallest_f) || (smallest_f == -1.0) )
{
smallest_f = pCurrent->f;
pChosen = pCurrent;
}
}
if ( !pChosen )
{
m_bSearching = false;
return;
}
// Check if this node is the goal
if (pChosen->index == m_iEndNodeIndex)
{
// The path has been found!
astar_t *pCurrNode = pChosen;
m_pResult = new CList<CNode>;
while ( pCurrNode )
{
m_pResult->InsertAtTail( pCurrNode->pNode );
if ( pCurrNode->parent != -1 )
pCurrNode = &m_Nodes[pCurrNode->parent];
else
pCurrNode = NULL;
}
m_bSearching = false;
return;
}
CList<CNode> *pPaths = NULL;
if (pChosen->pNode)
pPaths = pChosen->pNode->GetPaths();
else
continue;
if (!pPaths)
continue;
// Mark this node as closed
pChosen->closed = true;
// Remove from open list
m_pOpenList->Remove( pChosen );
pChosen->open = false;
// Loop through the possible successors
for ( i = 0; i < pPaths->GetItemCount(); i++ )
{
CNode *pNode = pPaths->Get(i);
astar_t *pCurr = &m_Nodes[pNode->GetIndex()];
if (!pNode)
continue;
// If this node is linked to itself for some unknown reason, skip it.
if ( pNode == pChosen->pNode )
continue;
// Calculate the new cost.
int new_cost = pChosen->cost + g_Util.GetDistanceBetween( pChosen->pNode->GetOrigin(), pNode->GetOrigin() );
// If this node is not touched yet OR if a better route has been found...
if (((!pCurr->closed) && (!pCurr->open)) ||
(((pCurr->open) || (pCurr->closed)) && (new_cost < pCurr->cost)))
{
m_pOpenList->Remove( pCurr );
pCurr->cost = new_cost;
pCurr->heuristic = g_Util.GetDistanceBetween( m_Nodes[m_iEndNodeIndex].pNode->GetOrigin(), pNode->GetOrigin() );
pCurr->f = pCurr->cost + pCurr->heuristic;
pCurr->parent = pChosen->index;
pCurr->open = true;
m_pOpenList->Insert( pCurr );
}
}
}
}
CList<CNode>* CPathfinder::GetResult( )
{
if (m_bSearching)
{
g_Output.Log( LOG_DEV, "CPathfinder::GetResult(): ERROR: Search in progress." );
return NULL;
}
return m_pResult;
}
EDIT: Code tags are back.