math/astar.cpp

Go to the documentation of this file.

#pragma warning( disable : 4786 )

#include "astar.h"
#include "debug/debug.h"

namespace Graph
{

//=============================================================================
// AddToOpenList
//
// adds a node to the open list
//=============================================================================
void Astar::AddToOpenList( const int nodeNumber )
{
    m_OpenList.insert( nodeNumber );
}

//=============================================================================
// GetCost
//
// gets the cost of a given node to the start
//=============================================================================
double Astar::GetCost( const int nodeNumber )
{
    double returnMe = m_Costs[ nodeNumber ];
    return returnMe;
}

//=============================================================================
// GetNodeFromOpenList
//
// plucks a node out of the open list
//=============================================================================
int Astar::GetNodeFromOpenList() const
{
    int returnMe = *( m_OpenList.begin() );
    return returnMe;
}

//=============================================================================
// Astar::GetPath
//
// returns the path from the search
//=============================================================================
std::vector< int > Astar::GetPath() const
{
    return m_Path;
}

//=============================================================================
// MarkNodeCost
//
// marks a node with a given cost
//=============================================================================
void Astar::MarkNodeCost( const int nodeNumber, const double cost )
{
    m_Costs[ nodeNumber ] = cost;
}

//=============================================================================
// OpenListIsEmpty
//
// checks if the open list is empty
//=============================================================================
bool Astar::OpenListIsEmpty() const
{
    bool returnMe = m_OpenList.empty();
    return returnMe;
}

//=============================================================================
// ProcessAllNeighborsOfNode
//
// 
//=============================================================================
void Astar::ProcessAllNeighborsOfNode( const GraphBase& g, const int node )
{
    double currentCost = GetCost( node );

    std::vector< int > neighbors = g.GetNeighbors( node );
    int size = neighbors.size();
    int i;
    for( i = 0; i < size; ++i )
    {
        const int neighbor = neighbors[ i ];
        double currentNeighborCost = GetCost( neighbor );
        if( currentNeighborCost > currentCost + 1 )
        {
            MarkNodeCost( neighbor, currentCost + 1 );
            AddToOpenList( neighbor );
        }
    }
}

//=============================================================================
// RemoveNodeFromOpenList
//
// removes a node from the open list
//=============================================================================
void Astar::RemoveNodeFromOpenList( const int nodeNum )
{
    m_OpenList.erase( nodeNum );
}

//=============================================================================
// Search
//
// performs the astar search on the graph
//=============================================================================
void Astar::Search( const GraphBase& g, const int start, const int end )
{
    //
    // Set up the sizes of the cost vector
    //
    size_t numNodes = g.GetNumNodes();
    m_Costs.resize( numNodes, 10000.0 );


    //
    // add the start node to the open list
    //
    AddToOpenList( end );
    MarkNodeCost( end, 0.0 );

    while( !OpenListIsEmpty() )
    {
        int node = GetNodeFromOpenList();
        RemoveNodeFromOpenList( node );
        ProcessAllNeighborsOfNode( g, node );
    }

    //
    // Lets check if the start and goal are colored
    //
    double startCost = GetCost( start );
    double endCost   = GetCost( end   );
    IJG_Assert( startCost < 10000.0 );
    IJG_Assert( endCost   < 10000.0 );

    if( startCost >= 10000.0 )
    {
        return;
    }
    if( endCost >= 10000.0 )
    {
        return;
    }

    //
    // all the nodes should be colored now
    //
    m_Path.clear();
    m_Path.push_back( start );

    int smallestNeighbor = SmallestNeighbor( g, start );
    while( smallestNeighbor != end )
    {
        m_Path.push_back( smallestNeighbor );
        smallestNeighbor = SmallestNeighbor( g, smallestNeighbor );
    }
    m_Path.push_back( end );
}

//=============================================================================
// SmallestNeighbor
//
// determines the smallest neighbor of a node in the colored graph
//=============================================================================
int Astar::SmallestNeighbor( const GraphBase& g, const int node )
{
    std::vector< int > neighbors = g.GetNeighbors( node );
    int size = neighbors.size();
    int i;

    int smallestNeighbor = neighbors[ 0 ];
    double smallestCost = m_Costs[ smallestNeighbor ];
    for( i = 1; i < size; ++i )
    {
        int neighbor = neighbors[ i ];
        double cost  = m_Costs[ neighbor ];
        if( cost < smallestCost )
        {
            smallestCost = cost;
            smallestNeighbor = neighbor;
        }
    }
    return smallestNeighbor;
}

} //namespace Graph

---