MapSearchAStar.cpp

/*----------------------------------------------------------------------
Pac-Man Evolution - Roberto Prieto
Copyright (C) 2018-2024 MegaStorm Systems
contact@megastormsystems.com - http://www.megastormsystems.com
 
This software is provided 'as-is', without any express or implied
warranty.  In no event will the authors be held liable for any damages
arising from the use of this software.
 
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
 
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
 
------------------------------------------------------------------------
 
MapSearch A* class
 
------------------------------------------------------------------------ */
 
#include "MapSearchAStar.h"
#include "GameField.h"
#include <algorithm>
#include "Pac-Man_Evolution.h"
 
// Class MazeNode (hidden for users)
class MazeNode : public CMemPME
{
public:
    MazeNode();
    MazeNode(Sint32, Sint32);
    bool isGoal(MazeNode*);
    bool isSameState(MazeNode*);
    void info();
 
public:
    Uint32 iX;
    Uint32 iY;
    MazeNode *pParent;
    MazeNode *pChild;
 
    float g; // Cost of this node plus all ancestors
    float h; // Heuristic estimation to the target
    float f; // Sum of g + h    
};
 
class HeapCompare_f
{
public:
    bool operator() (const MazeNode* n1, const MazeNode* n2) const { return n1->f > n2->f; }
};
 
// Internal defines
#define COST_ORTHOGONAL 1.0f
//#define DEBUG_MAPSEARCH
 
// --- MazeNode class implementation ---
MazeNode::MazeNode()
{
    pParent = pChild = nullptr;
    g = h = f = 0.0f;
    iX = iY = 0;
}
 
MazeNode::MazeNode(Sint32 iNX, Sint32 iNY)
{
    pParent = pChild = nullptr;
    h = f = g = 0.0f;
    iX = iNX; iY = iNY;
}
 
bool MazeNode::isSameState(MazeNode* pNode)
{
    if((iX == pNode->iX) && (iY == pNode->iY)) return true;
    else return false;
}
 
bool MazeNode::isGoal(MazeNode* pNode)
{
    if((iX == pNode->iX) && (iY == pNode->iY)) return true;
    return false;
}
 
void MazeNode::info()
{
    Main::Instance().ILogMgr().get()->msg(LML_NORMAL, "   [MazeNode] Info: (%d,%d) f(%.3f)=g(%.3f)+h(%.3f)\n", iX, iY, f, g, h);
}
 
// --- MapSearchAStar class implementation ---
MapSearchAStar::MapSearchAStar(GameField* GF)
{
    vOpenList.clear();
    vClosedList.clear();
    vSuccessors.clear();
    iAStarSteps = 0;
    eSS = SS_NOT_INITIALISED;
    pNodeStart = nullptr;
    pNodeGoal = nullptr;
    pNodeCurrentSolution = nullptr;
    iMinimumAcceptableValue = 0;
    bCancelRequest = false;
 
    // Debug
    iAllocateNodeCount = 0;
    iFreeNodeCount = 0;
    
    // Pointer to needed components
    pGameField = GF;
}
 
MapSearchAStar::~MapSearchAStar()
{
    pGameField = nullptr;
}
 
// Return a path using A* algorithm
MapSearchAStar::eSearchState MapSearchAStar::findPath(Sint32 iXStart, Sint32 iYStart, Sint32 iXGoal, Sint32 iYGoal, vector<MazePoint>& pPath, Sint32 iValid)
{
    // Check that current position and destination are not the same
    if(iXStart == iXGoal && iYStart == iYGoal) return SS_INVALID;
 
    // Init some vars
    iMinimumAcceptableValue = iValid;
    iAStarSteps = 0;
    eSS = SS_SEARCHING;
 
    // Allocate start and goal nodes
    iAllocateNodeCount = 0;
    iFreeNodeCount = 0;
    bCancelRequest = false;
    pNodeStart = allocateNode();
    pNodeGoal = allocateNode();
 
    // Set start and goal nodes
    pNodeStart->iX = iXStart; pNodeStart->iY = iYStart;
    pNodeGoal->iX = iXGoal;  pNodeGoal->iY = iYGoal;
    pNodeStart->g = 0;
    pNodeStart->h = heuristic(pNodeStart);
    pNodeStart->f = pNodeStart->g + pNodeStart->h;
    pNodeStart->pParent = 0;
 
    // Push start node to the open list
    vOpenList.push_back(pNodeStart);          
 
    // Main A* loop
    do
    {
        AStarDoStep();
        ++iAStarSteps;
    } while(eSS == SS_SEARCHING);
 
    // If the search succeeded, return the path
    if(eSS == SS_SUCCEEDED)
    {
        MazeNode* node = getSolutionStart();
        MazePoint point;
        for(;; )
        {
            node = getSolutionNext();
            if(!node) break;
            point.iX = node->iX;
            point.iY = node->iY;
            pPath.push_back(point);
            //node->info();            
        };      
        // First element of the vector is the target so we are "poping" elements from the vector
        std::reverse(pPath.begin(), pPath.end()); 
        freeSolutionNodes();
    }
 
    // Show info
    #ifdef DEBUG_MAPSEARCH     
      Main::Instance().ILogMgr().get()->msg(LML_NORMAL, "   [MapSearchA*] Info: iterations %d - Allocated/free nodes %d/%d - ", iAStarSteps, iAllocateNodeCount, iFreeNodeCount);
      if(eSS == SS_FAILED) Main::Instance().ILogMgr().get()->msg(LML_NORMAL, "Goal node not found\n");
      else Main::Instance().ILogMgr().get()->msg(LML_NORMAL, "Goal node found with %d steps\n", pPath.size());
    #endif
    return eSS;
}
 
// Heuristic method: estimation to the goal node using Manhattan distance as we allow to move in 4 directions
float MapSearchAStar::heuristic(MazeNode* pNode)
{
    float fHeuristic;
    float D, cross;
 
    // Better to set it to close to the lowest cost between two tiles
    D = 1.2f;
 
    //  Manhattan distance
    float dx = abs((float)pNode->iX - (float)pNodeGoal->iX);
    float dy = abs((float)pNode->iY - (float)pNodeGoal->iY);
    fHeuristic = D * (dx + dy);
 
    // Breaking ties: prefer paths that are along the straight line from start to goal nodes
    float dx1 = (float)pNode->iX - (float)pNodeGoal->iX;
    float dy1 = (float)pNode->iY - (float)pNodeGoal->iY;
    float dx2 = (float)pNodeStart->iX - (float)pNodeGoal->iX;
    float dy2 = (float)pNodeStart->iY - (float)pNodeGoal->iY;
    cross = abs(dx1 * dy2 - dx2 * dy1);
 
    fHeuristic = fHeuristic + cross * 0.001f;
    return fHeuristic;
}
 
// Get the cost of moving to a node
float MapSearchAStar::cost(MazeNode *successor, Sint32 x, Sint32 y)
{
    float fCost = COST_ORTHOGONAL;
 
    // ToDO: with pellets we could decrease the cost
    return fCost;
}
 
// A* algorithm running step by step
void MapSearchAStar::AStarDoStep()
{
    // If we succeeded or failed, return
    if((eSS == SS_SUCCEEDED) || (eSS == SS_FAILED)) return;
 
    // If no more opened nodes or a cancel request, return
    if(vOpenList.empty() || bCancelRequest)
    {
        freeAllNodes();
        eSS = SS_FAILED;
        return;
    }
 
    // Get the best node till now (lowest f)
    MazeNode* n = vOpenList.front();
    pop_heap(vOpenList.begin(), vOpenList.end(), HeapCompare_f());
    vOpenList.pop_back();
 
    // Is it the goal?
    if(n->isGoal(pNodeGoal))
    {
        // The user is going to use the Goal Node he passed in so copy the parent pointer of n 
        pNodeGoal->pParent = n->pParent;
 
        // A special case is that the goal was passed in as the start state so handle that here
        if(n != pNodeStart)
        {
            freeNode(n);
 
            // set the child pointers in each node (except Goal which has no child)
            MazeNode* nodeChild = pNodeGoal;
            MazeNode* nodeParent = pNodeGoal->pParent;
 
            do
            {
                nodeParent->pChild = nodeChild;
                nodeChild = nodeParent;
                nodeParent = nodeParent->pParent;
            } while(nodeChild != pNodeStart); // Start is always the first node by definition
        }
 
        // Free non-used nodes on current solution
        freeUnusedNodes();
        eSS = SS_SUCCEEDED;
        return;
    }
 
    // No, keep searching
    else
    {
        // Get new successors
        vSuccessors.clear();
        bool ret = getSuccessors(n, n->pParent);
        if(!ret)
        {
            // free the nodes that may previously have been added 
            for(vector< MazeNode * >::iterator successor = vSuccessors.begin(); successor != vSuccessors.end(); ++successor)
            {
                freeNode((*successor));
            }
 
            vSuccessors.clear(); // empty vector of successor nodes to n
 
            // free up everything else we allocated
            freeAllNodes();
 
            eSS = SS_OUT_OF_MEMORY;
            return;
        }
 
        // Look for the best successor node
        for(vector< MazeNode * >::iterator successor = vSuccessors.begin(); successor != vSuccessors.end(); ++successor)
        {
            float newg = n->g + cost((*successor), n->iX, n->iY);
 
            // Now we need to find whether the node is on the open or closed lists
            // If it is but the node that is already on them is better (lower g)
            // then we can forget about this successor
 
            // First linear search of open list to find node
            vector< MazeNode * >::iterator openlist_result;
            for(openlist_result = vOpenList.begin(); openlist_result != vOpenList.end(); ++openlist_result)
            {
                if((*openlist_result)->isSameState((*successor))) break;
            }
            if(openlist_result != vOpenList.end())
            {
                // we found this state on open
                if((*openlist_result)->g <= newg)
                {
                    freeNode((*successor));
                    // the one on Open is cheaper than this one
                    continue;
                }
            }
 
            vector< MazeNode * >::iterator closedlist_result;
            for(closedlist_result = vClosedList.begin(); closedlist_result != vClosedList.end(); ++closedlist_result)
            {
                if((*closedlist_result)->isSameState((*successor))) break;
            }
 
            if(closedlist_result != vClosedList.end())
            {
                // we found this state on closed
                if((*closedlist_result)->g <= newg)
                {
                    // the one on Closed is cheaper than this one
                    freeNode((*successor));
                    continue;
                }
            }
 
            // This node is the best node so far with this particular state so lets keep it and set up its AStar specific data ...
            (*successor)->pParent = n;
            (*successor)->g = newg;
            (*successor)->h = heuristic((*successor));
            (*successor)->f = (*successor)->g + (*successor)->h;
 
            // Remove successor from closed if it was on it
            if(closedlist_result != vClosedList.end())
            {
                // remove it from Closed
                freeNode((*closedlist_result));
                vClosedList.erase(closedlist_result);
            }
 
            // Update old version of this node
            if(openlist_result != vOpenList.end())
            {
                freeNode((*openlist_result));
                vOpenList.erase(openlist_result);
 
                // re-make the heap 
                make_heap(vOpenList.begin(), vOpenList.end(), HeapCompare_f());
            }
 
            // heap now unsorted
            vOpenList.push_back((*successor));
 
            // sort back element into heap
            push_heap(vOpenList.begin(), vOpenList.end(), HeapCompare_f());
        }
 
        // push n onto Closed, as we have expanded it now
        vClosedList.push_back(n);
    }
    return;
}
 
// Add a successor to the list of succesors
bool MapSearchAStar::addSuccessor(MazeNode* State)
{
    MazeNode* node = allocateNode();
    if(node)
    {
        *node = *State;
        vSuccessors.push_back(node);
        return true;
    }
    return false;
}
 
// Add all posible successors to the current node
// Check the Gamefield state looking for wakable cells
bool MapSearchAStar::getSuccessors(MazeNode *current, MazeNode *parent_node)
{
    MazeNode NewNode;
    Sint32 parent_x = -1;
    Sint32 parent_y = -1;
 
    if(parent_node)
    {
        parent_x = parent_node->iX;
        parent_y = parent_node->iY;
    }
 
    // Left node
    if((pGameField->getState(current->iX - 1, current->iY) >= iMinimumAcceptableValue) && !((parent_x == current->iX - 1) && (parent_y == current->iY)))
    {
        NewNode = MazeNode(current->iX - 1, current->iY);
        addSuccessor(&NewNode);
    }
    // Right node
    if((pGameField->getState(current->iX + 1, current->iY) >= iMinimumAcceptableValue) && !((parent_x == current->iX + 1) && (parent_y == current->iY)))
    {
        NewNode = MazeNode(current->iX + 1, current->iY);
        addSuccessor(&NewNode);
    }
    // Up node
    if((pGameField->getState(current->iX, current->iY - 1) >= iMinimumAcceptableValue) && !((parent_x == current->iX) && (parent_y == current->iY - 1)))
    {
        NewNode = MazeNode(current->iX, current->iY - 1);
        addSuccessor(&NewNode);
    }
    // Down node
    if((pGameField->getState(current->iX, current->iY + 1) >= iMinimumAcceptableValue) && !((parent_x == current->iX) && (parent_y == current->iY + 1)))
    {
        NewNode = MazeNode(current->iX, current->iY + 1);
        addSuccessor(&NewNode);
    }
    return true;
}
 
// Cancel the search
void MapSearchAStar::cancelSearch()
{
    bCancelRequest = true;
}
 
// Create a node
MazeNode* MapSearchAStar::allocateNode()
{
    iAllocateNodeCount++;
    MazeNode* p = new(std::nothrow) MazeNode;
    return p;
}
 
// Destroy a node
void MapSearchAStar::freeNode(MazeNode* node)
{
    iFreeNodeCount++;
    delete node;
}
 
// Free all used nodes
void MapSearchAStar::freeAllNodes()
{
    // Free nodes on the opened list
    vector<MazeNode*>::iterator iterOpen = vOpenList.begin();
    while(iterOpen != vOpenList.end())
    {
        MazeNode* n = (*iterOpen);
        freeNode(n);
        ++iterOpen;
    }
    vOpenList.clear();
 
    // Free nodes on the closed list
    vector<MazeNode*>::iterator iterClosed;
    for(iterClosed = vClosedList.begin(); iterClosed != vClosedList.end(); ++iterClosed)
    {
        MazeNode* n = (*iterClosed);
        freeNode(n);
    }
    vClosedList.clear();
}
 
// Free all non-used nodes
void MapSearchAStar::freeUnusedNodes()
{
    // Remove all non-used nodes on the opened list
    vector<MazeNode*>::iterator iterOpen = vOpenList.begin();
    while(iterOpen != vOpenList.end())
    {
        MazeNode* n = (*iterOpen);
        if(!n->pChild) { freeNode(n); n = nullptr; }
        ++iterOpen;
    }
    vOpenList.clear();
 
    // Remove all non-used nodes on the closed list
    vector<MazeNode*>::iterator iterClosed;
    for(iterClosed = vClosedList.begin(); iterClosed != vClosedList.end(); ++iterClosed)
    {
        MazeNode* n = (*iterClosed);
        if(!n->pChild) { freeNode(n); n = nullptr; }
    }
    vClosedList.clear();
}
 
// Free nodes used on the solution
void MapSearchAStar::freeSolutionNodes()
{
    MazeNode* n = pNodeStart;
    if(pNodeStart->pChild)
    {
        do
        {
            MazeNode* del = n;
            n = n->pChild;
            freeNode(del);
            del = nullptr;
        } while(n != pNodeGoal);
 
        freeNode(n); 
    }
    else
    {
        // Start and goal nodes are the same, only remove both
        freeNode(pNodeStart);
        freeNode(pNodeGoal);
    }
 
}
 
// Get solution start node
MazeNode* MapSearchAStar::getSolutionStart()
{
    pNodeCurrentSolution = pNodeStart;
    if(pNodeStart)  return pNodeStart;
    else return nullptr;
}
 
// Get solution next node
MazeNode* MapSearchAStar::getSolutionNext()
{
    if(pNodeCurrentSolution)
    {
        if(pNodeCurrentSolution->pChild)
        {
            MazeNode* child = pNodeCurrentSolution->pChild;
            pNodeCurrentSolution = pNodeCurrentSolution->pChild;
            return child;
        }
    }
    return nullptr;
}
 
// Get solution last node
MazeNode* MapSearchAStar::getSolutionEnd()
{
    pNodeCurrentSolution = pNodeGoal;
    if(pNodeGoal) return pNodeGoal;
    else return nullptr;
}
 
// Get solution previous node
MazeNode* MapSearchAStar::getSolutionPrev()
{
    if(pNodeCurrentSolution)
    {
        if(pNodeCurrentSolution->pParent)
        {
            MazeNode* parent = pNodeCurrentSolution->pParent;
            pNodeCurrentSolution = pNodeCurrentSolution->pParent;
            return parent;
        }
    }
    return nullptr;
}
 
// Debugging methods
 
// Get opened list start node
MazeNode* MapSearchAStar::getOpenListStart()
{
    float f, g, h;
    return getOpenListStart(f, g, h);
}
MazeNode* MapSearchAStar::getOpenListStart(float &f, float &g, float &h)
{
    iterDbgOpen = vOpenList.begin();
    if(iterDbgOpen != vOpenList.end())
    {
        f = (*iterDbgOpen)->f;
        g = (*iterDbgOpen)->g;
        h = (*iterDbgOpen)->h;
        return (*iterDbgOpen);
    }
    return nullptr;
}
 
// Get opened list next node
MazeNode* MapSearchAStar::getOpenListNext()
{
    float f, g, h;
    return getOpenListNext(f, g, h);
}
MazeNode* MapSearchAStar::getOpenListNext(float &f, float &g, float &h)
{
    ++iterDbgOpen;
    if(iterDbgOpen != vOpenList.end())
    {
        f = (*iterDbgOpen)->f;
        g = (*iterDbgOpen)->g;
        h = (*iterDbgOpen)->h;
        return (*iterDbgOpen);
    }
    return nullptr;
}
 
// Get closed list start node
MazeNode* MapSearchAStar::getClosedListStart()
{
    float f, g, h;
    return getClosedListStart(f, g, h);
}
MazeNode* MapSearchAStar::getClosedListStart(float &f, float &g, float &h)
{
    iterDbgClosed = vClosedList.begin();
    if(iterDbgClosed != vClosedList.end())
    {
        f = (*iterDbgClosed)->f;
        g = (*iterDbgClosed)->g;
        h = (*iterDbgClosed)->h;
        return (*iterDbgClosed);
    }
    return nullptr;
}
 
// Get closed list next node
MazeNode* MapSearchAStar::getClosedListNext()
{
    float f, g, h;
    return getClosedListNext(f, g, h);
}
MazeNode* MapSearchAStar::getClosedListNext(float &f, float &g, float &h)
{
    ++iterDbgClosed;
    if(iterDbgClosed != vClosedList.end())
    {
        f = (*iterDbgClosed)->f;
        g = (*iterDbgClosed)->g;
        h = (*iterDbgClosed)->h;
        return (*iterDbgClosed);
    }
    return nullptr;
}