GDevelop/Extensions/PathfindingBehavior/PathfindingBehavior.cpp

/**

GDevelop - Pathfinding Behavior Extension
Copyright (c) 2010-2015 Florian Rival (Florian.Rival@gmail.com)
This project is released under the MIT License.
*/

#include <memory>
#include <unordered_map>
#include <iostream>
#include <set>
#include "PathfindingBehavior.h"
#include "PathfindingObstacleBehavior.h"
#include "ScenePathfindingObstaclesManager.h"
#include "GDCore/Tools/Localization.h"
#include "GDCpp/BuiltinExtensions/MathematicalTools.h"
#include "GDCpp/Scene.h"
#include "GDCpp/Serialization/SerializerElement.h"
#include "GDCpp/RuntimeScene.h"
#include "GDCpp/RuntimeObject.h"
#include "GDCpp/CommonTools.h"
#include <iostream>
#include <cmath>
#include <algorithm>
#if defined(GD_IDE_ONLY)
#include <map>
#include "GDCore/IDE/Dialogs/PropertyDescriptor.h"
#endif


/**
 * \brief Internal tool class representing the position of a node when looking for a path.
 */
class NodePosition
{
public:
    NodePosition(int x_, int y_) : x(x_), y(y_) {};

    int x;
    int y;
};

std::ostream& operator<<(std::ostream& stream, const NodePosition & nodePos)
{
    stream << nodePos.x << ";" << nodePos.y;
    return stream;
}

bool operator==(const NodePosition &a, const NodePosition &b)
{
    return ((a.x == b.x) && (a.y == b.y));
}

namespace std
{
/**
 * \brief Tool function used to store a NodePosition as key in std::unordered_set.
 */
template<>
struct hash<NodePosition>
{
    std::size_t operator()(NodePosition const & n) const
    {
        return (std::hash<int>()(n.x)) ^ (std::hash<int>()(n.y) << 1);
    }
};

}

namespace
{
/**
 * \brief Internal tool class representing a node when looking for a path
 */
class Node
{
public:
    Node() : pos(0, 0), cost(0), smallestCost(-1), estimateCost(-1), parent(NULL), open(true) {};
    Node(int x, int y) : pos(x, y), cost(0), smallestCost(-1), estimateCost(-1), parent(NULL), open(true) {};
    Node(const NodePosition & pos_) : pos(pos_), cost(0), smallestCost(-1), estimateCost(-1), parent(NULL), open(true) {};

    NodePosition pos;
    float cost; ///< The cost for traveling on this node
    float smallestCost; ///< the cost to go to this node (when considering the shortest path).
    float estimateCost; ///< the estimate cost total to go to the destination through this node (when considering the shortest path).
    const Node * parent; ///< The previous node to be visited to go to this node (when considering the shortest path).
    bool open; ///< true if the node is "open" (must be explored), false if "close" (already explored)

    /**
     * \brief Tool function used to store a Node in a priority_queue.
     */
    class NodeComparator
    {
    public:
        bool operator()(const Node * n1, const Node * n2)
        {
            return n1->estimateCost < n2->estimateCost;
        }
    };
};

bool operator==(Node const& n1, Node const& n2)
{
    return n1.pos.x == n2.pos.x && n1.pos.y == n2.pos.y;
};

typedef float (*DistanceFunPtr)(const NodePosition & , const NodePosition & );

/**
 * \brief Internal tool class containing the structures used by A* and members functions related
 * to them.
 */
class SearchContext
{
public:
    SearchContext(ScenePathfindingObstaclesManager & obstacles_, bool allowsDiagonal_ = true) :
        obstacles(obstacles_),
        finalNode(NULL),
        destination(0, 0),
        startX(0),
        startY(0),
        allowsDiagonal(allowsDiagonal_),
        maxComplexityFactor(50),
        cellWidth(20),
        cellHeight(20),
        leftBorder(0),
        rightBorder(0),
        topBorder(0),
        bottomBorder(0)
    {
        distanceFunction = allowsDiagonal ? &SearchContext::EuclideanDistance : &SearchContext::ManhattanDistance;
    }

    /**
     * \brief Set the start position.
     * \param x The coordinate on X axis of the start position, in "world" coordinates.
     * \param y The coordinate on Y axis of the start position, in "world" coordinates.
     */
    SearchContext & SetStartPosition(float x, float y)
    {
        startX = x;
        startY = y;
        return *this;
    }

    /**
     * \brief Set the size to be considered for the object for which the path will be planned.
     */
    SearchContext & SetObjectSize(float leftBorder_, float topBorder_, float rightBorder_, float bottomBorder_)
    {
        leftBorder = leftBorder_;
        rightBorder = rightBorder_;
        topBorder = topBorder_;
        bottomBorder = bottomBorder_;
        return *this;
    }

    /**
     * \brief Change the size of a virtual cell, in pixels.
     */
    SearchContext & SetCellSize(unsigned int cellWidth_, unsigned int cellHeight_)
    {
        cellWidth = cellWidth_;
        cellHeight = cellHeight_;
        return *this;
    }

    /**
     * \brief Compute a path to the specified position, considering the obstacles
     * and the start position passed in the constructor.
     * \return true if computation found a path, in which case you can call GetFinalNode method
     * to construct the path.
     * \param x The coordinate on X axis of the target position, in "world" coordinates.
     * \param y The coordinate on Y axis of the target position, in "world" coordinates.
     */
    bool ComputePathTo(float targetX, float targetY)
    {
        destination = NodePosition(GDRound(targetX/cellWidth), GDRound(targetY/cellHeight));
        NodePosition start(GDRound(startX/cellWidth), GDRound(startY/cellHeight));

        //Initialize the algorithm
        allNodes.clear();
        Node & startNode = GetNode(start);
        startNode.smallestCost = 0;
        startNode.estimateCost = 0 + distanceFunction(start, destination);
        openNodes.clear();
        openNodes.insert(&startNode);

        //A* algorithm main loop
        std::size_t iterationCount = 0;
        std::size_t maxIterationCount = startNode.estimateCost*maxComplexityFactor;
        while (!openNodes.empty())
        {
            if (iterationCount++ > maxIterationCount) return false; //Make sure we do not search forever.

            Node * n = *openNodes.begin(); //Get the most promising node...
            n->open = false;                 //...and flag it as explored
            openNodes.erase(openNodes.begin()); //Be sure to remove ONLY the first element!

            //Check if we reached destination?
            if ( n->pos.x == destination.x && n->pos.y == destination.y )
            {
                finalNode = n;
                return true;
            }

            //No, so add neighbors to the nodes to explore.
            InsertNeighbors(*n);
        }

        return false;
    }

    /**
     * @return The final node of the computed path.
     * Iterate on the parent member to create the path. Beware, the coordinates of the node
     * must be multiplied by the cell size to get the "world" coordinates of the path.
     */
    Node * GetFinalNode() const
    {
        return finalNode;
    }

    #if 0
    static void UnitTest()
    {
        {
            ScenePathfindingObstaclesManager obstacles;
            SearchContext ctx(obstacles, 0, 0);
            ctx.ComputePathTo(0,0);
        }
        std::cout << "---" << std::endl;
        {
            ScenePathfindingObstaclesManager obstacles;
            SearchContext ctx(obstacles, 0, 0);
            ctx.ComputePathTo(1,0);
        }
        std::cout << "---" << std::endl;
        {
            ScenePathfindingObstaclesManager obstacles;
            SearchContext ctx(obstacles, 0, 0);
            ctx.ComputePathTo(0,1);
        }
        std::cout << "---" << std::endl;
        {
            ScenePathfindingObstaclesManager obstacles;
            SearchContext ctx(obstacles, 0, 0);
            ctx.ComputePathTo(1,1);
        }
        std::cout << "---" << std::endl;
        {
            ScenePathfindingObstaclesManager obstacles;
            SearchContext ctx(obstacles, 3, 4);
            ctx.ComputePathTo(12,9);
        }
        std::cout << "---" << std::endl;
        std::cout << "End unit tests";
    }
    #endif

private:
    /**
     * Insert the neighbors of the current node in the open list
     * (Only if they are not closed, and if the cost is better than the already existing smallest cost).
     */
    void InsertNeighbors(const Node & currentNode)
    {
        AddOrUpdateNode(NodePosition(currentNode.pos.x+1, currentNode.pos.y), currentNode, 1);
        AddOrUpdateNode(NodePosition(currentNode.pos.x-1, currentNode.pos.y), currentNode, 1);
        AddOrUpdateNode(NodePosition(currentNode.pos.x, currentNode.pos.y+1), currentNode, 1);
        AddOrUpdateNode(NodePosition(currentNode.pos.x, currentNode.pos.y-1), currentNode, 1);
        if ( allowsDiagonal )
        {
            AddOrUpdateNode(NodePosition(currentNode.pos.x+1, currentNode.pos.y+1), currentNode, sqrt2);
            AddOrUpdateNode(NodePosition(currentNode.pos.x+1, currentNode.pos.y-1), currentNode, sqrt2);
            AddOrUpdateNode(NodePosition(currentNode.pos.x-1, currentNode.pos.y-1), currentNode, sqrt2);
            AddOrUpdateNode(NodePosition(currentNode.pos.x-1, currentNode.pos.y+1), currentNode, sqrt2);
        }
    }

    /**
     * \brief Get (or dynamically construct) a node.
     *
     * *All* nodes should be created using this method: The cost of the node is computed thanks
     * to the objects flagged as obstacles.
     */
    Node & GetNode(const NodePosition & pos)
    {
        if (allNodes.find(pos) != allNodes.end())
            return allNodes.find(pos)->second;

        Node newNode(pos);

        bool objectsOnCell = false;
        const std::set<PathfindingObstacleBehavior*> & allObstacles = obstacles.GetAllObstacles();
        for (std::set<PathfindingObstacleBehavior*>::const_iterator it = allObstacles.begin();
             it != allObstacles.end();
             ++it)
        {
            RuntimeObject * obj = (*it)->GetObject();
            int topLeftCellX = floor((obj->GetDrawableX()-rightBorder)/(float)cellWidth);
            int topLeftCellY = floor((obj->GetDrawableY()-bottomBorder)/(float)cellHeight);
            int bottomRightCellX = ceil((obj->GetDrawableX()+obj->GetWidth()+leftBorder)/(float)cellWidth);
            int bottomRightCellY = ceil((obj->GetDrawableY()+obj->GetHeight()+topBorder)/(float)cellHeight);
            if ( topLeftCellX <= pos.x && pos.x < bottomRightCellX
                && topLeftCellY <= pos.y && pos.y < bottomRightCellY)
            {
                objectsOnCell = true;
                if ( (*it)->IsImpassable() )
                {
                    newNode.cost = -1;
                    break; //The cell is impassable, stop here.
                }
                else //Superimpose obstacles
                    newNode.cost += (*it)->GetCost();
            }
        }

        if (!objectsOnCell) newNode.cost = 1; //Default cost when no objects put on the cell.

        allNodes[pos] = newNode;
        return allNodes[pos];
    }

    /**
     * Compute the euclidean distance between two positions.
     */
    static float EuclideanDistance(const NodePosition & a, const NodePosition & b)
    {
        return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
    }

    /**
     * Compute the taxi distance between two positions.
     */
    static float ManhattanDistance(const NodePosition & a, const NodePosition & b)
    {
        return abs(a.x-b.x)+abs(a.y-b.y);
    }

    /**
     * Add a node to the openNodes (only if the cost to reach it is less than the existing cost, if any).
     */
    void AddOrUpdateNode(const NodePosition & newNodePosition, const Node & currentNode, float factor)
    {
        Node & neighbor = GetNode(newNodePosition);
        if (!neighbor.open || neighbor.cost < 0 ) //cost < 0 means impassable obstacle
            return;

        //Update the node costs and parent if the path coming from currentNode is better:
        if (neighbor.smallestCost == -1
            || neighbor.smallestCost > currentNode.smallestCost + (currentNode.cost+neighbor.cost)/2.0*factor)
        {
            if (neighbor.smallestCost != -1) //The node is already in the open list:
                openNodes.erase(&neighbor);   //remove it as its estimate cost will be updated.

            neighbor.smallestCost = currentNode.smallestCost + (currentNode.cost+neighbor.cost)/2.0*factor;
            neighbor.parent = &currentNode;
            neighbor.estimateCost = neighbor.smallestCost + distanceFunction(neighbor.pos, destination);

            openNodes.insert(&neighbor);
        }
    }

    std::unordered_map< NodePosition, Node > allNodes; ///< All the nodes
    std::multiset<Node*, Node::NodeComparator> openNodes; ///< Only the open nodes (Such that Node::open == true)
    const ScenePathfindingObstaclesManager & obstacles; ///< A reference to all the obstacles of the scene
    Node * finalNode; //If computation succeeded, the final node is stored here.
    NodePosition destination;
    int startX; ///< The start X position, in "world" coordinates (not in "node" coordinates!).
    int startY; ///< The start Y position, in "world" coordinates (not in "node" coordinates!).
    DistanceFunPtr distanceFunction;
    bool allowsDiagonal; ///< True to allow diagonals when planning the path.
    std::size_t maxComplexityFactor;
    float cellWidth;
    float cellHeight;
    float leftBorder;
    float rightBorder;
    float topBorder;
    float bottomBorder;

    static const float sqrt2;
};

const float SearchContext::sqrt2 = 1.414213562;

}

PathfindingBehavior::PathfindingBehavior() :
    parentScene(NULL),
    sceneManager(NULL),
    pathFound(false),
    allowDiagonals(true),
    acceleration(400),
    maxSpeed(200),
    angularMaxSpeed(180),
    rotateObject(true),
    angleOffset(0),
    cellWidth(20),
    cellHeight(20),
    extraBorder(0),
    speed(0),
    angularSpeed(0),
    timeOnSegment(0),
    totalSegmentTime(0),
    currentSegment(0),
    reachedEnd(false)
{
}

void PathfindingBehavior::MoveTo(RuntimeScene & scene, float x, float y)
{
    if ( parentScene != &scene ) //Parent scene has changed
    {
        parentScene = &scene;
        sceneManager = parentScene ? &ScenePathfindingObstaclesManager::managers[&scene] : NULL;
    }

    path.clear();

    //First be sure that there is a path to compute.
    int targetCellX = GDRound(x/(float)cellWidth);
    int targetCellY = GDRound(y/(float)cellHeight);
    int startCellX = GDRound(object->GetX()/(float)cellWidth);
    int startCellY = GDRound(object->GetY()/(float)cellHeight);
    if ( startCellX == targetCellX && startCellY == targetCellY ) {
        path.push_back(sf::Vector2f(object->GetX(), object->GetY()));
        path.push_back(sf::Vector2f(x, y));
        EnterSegment(0);
        pathFound = true;
        return;
    }

    //Start searching for a path
    //TODO: Customizable heuristic.
    ::SearchContext ctx(*sceneManager, allowDiagonals);
    ctx.SetCellSize(cellWidth, cellHeight).SetStartPosition(object->GetX(), object->GetY());
    ctx.SetObjectSize(object->GetX()-object->GetDrawableX()+extraBorder,
        object->GetY()-object->GetDrawableY()+extraBorder,
        object->GetWidth()-(object->GetX()-object->GetDrawableX())+extraBorder,
        object->GetHeight()-(object->GetY()-object->GetDrawableY())+extraBorder);
    if (ctx.ComputePathTo(x, y))
    {
        //Path found: memorize it
        const ::Node * node = ctx.GetFinalNode();
        while (node) {
            path.push_back(sf::Vector2f(node->pos.x*(float)cellWidth, node->pos.y*(float)cellHeight));
            node = node->parent;
        }

        std::reverse(path.begin(), path.end());
        path[0] = sf::Vector2f(object->GetX(), object->GetY());
        EnterSegment(0);
        pathFound = true;
        return;
    }

    //Not path found
    pathFound = false;
}

void PathfindingBehavior::EnterSegment(std::size_t segmentNumber)
{
    if ( path.empty() ) return;

    currentSegment = segmentNumber;
    if (currentSegment < path.size()-1)
    {
        sf::Vector2f newPath = (path[currentSegment + 1] - path[currentSegment]);
        totalSegmentTime = sqrtf(newPath.x*newPath.x+newPath.y*newPath.y);
        timeOnSegment = 0;
        reachedEnd = false;
    }
    else {
        reachedEnd = true;
        speed = 0;
    }
}

void PathfindingBehavior::DoStepPreEvents(RuntimeScene & scene)
{
    if ( parentScene != &scene ) //Parent scene has changed
    {
        parentScene = &scene;
        sceneManager = parentScene ? &ScenePathfindingObstaclesManager::managers[&scene] : NULL;
    }

    if ( !sceneManager ) return;

    if (path.empty() || reachedEnd) return;

    //Update the speed of the object
    float timeDelta = static_cast<double>(scene.GetElapsedTime())/1000000.0;
    speed += acceleration*timeDelta;
    if ( speed > maxSpeed ) speed = maxSpeed;
    angularSpeed = angularMaxSpeed; //No acceleration for angular speed for now

    //Update the time on the segment and change segment if needed
    timeOnSegment += speed*timeDelta;
    if (timeOnSegment >= totalSegmentTime && currentSegment < path.size())
        EnterSegment(currentSegment + 1);

    //Position object on the segment and update its angle
    sf::Vector2f newPos;
    float pathAngle = object->GetAngle();
    if ( currentSegment < path.size()-1 ) {
        newPos = path[currentSegment] + (path[currentSegment + 1] - path[currentSegment]) * (timeOnSegment / totalSegmentTime);
        pathAngle = atan2(path[currentSegment+1].y - path[currentSegment].y,
            path[currentSegment+1].x - path[currentSegment].x)*180/3.14159+angleOffset;
    }
    else
        newPos = path.back();

    object->SetX(newPos.x);
    object->SetY(newPos.y);

    //Also update angle if needed
    if ( rotateObject )
        object->RotateTowardAngle(pathAngle, angularSpeed, scene);
}

void PathfindingBehavior::DoStepPostEvents(RuntimeScene & scene)
{
    if ( parentScene != &scene ) //Parent scene has changed
    {
        parentScene = &scene;
        sceneManager = parentScene ? &ScenePathfindingObstaclesManager::managers[&scene] : NULL;
    }
}

float PathfindingBehavior::GetNodeX(std::size_t index) const
{
    if (index<path.size()) return path[index].x;
    return 0;
}
float PathfindingBehavior::GetNodeY(std::size_t index) const
{
    if (index<path.size()) return path[index].y;
    return 0;
}
std::size_t PathfindingBehavior::GetNextNodeIndex() const
{
    if (currentSegment+1 < path.size())
        return currentSegment+1;
    else
        return path.size()-1;
}
float PathfindingBehavior::GetNextNodeX() const
{
    if ( path.empty() ) return 0;

    if (currentSegment+1 < path.size())
        return path[currentSegment+1].x;
    else
        return path.back().x;
}
float PathfindingBehavior::GetNextNodeY() const
{
    if ( path.empty() ) return 0;

    if (currentSegment+1 < path.size())
        return path[currentSegment+1].y;
    else
        return path.back().y;
}
float PathfindingBehavior::GetLastNodeX() const
{
    if ( path.size() < 2 ) return 0;

    if (currentSegment < path.size()-1)
        return path[currentSegment].x;
    else
        return path[path.size()-1].x;
}
float PathfindingBehavior::GetLastNodeY() const
{
    if ( path.size() < 2 ) return 0;

    if (currentSegment < path.size()-1)
        return path[currentSegment].y;
    else
        return path[path.size()-1].y;
}
float PathfindingBehavior::GetDestinationX() const
{
    if ( path.empty() ) return 0;
    return path.back().x;
}
float PathfindingBehavior::GetDestinationY() const
{
    if ( path.empty() ) return 0;
    return path.back().y;
}

void PathfindingBehavior::UnserializeFrom(const gd::SerializerElement & element)
{
    allowDiagonals = element.GetBoolAttribute("allowDiagonals");
    acceleration = element.GetDoubleAttribute("acceleration");
    maxSpeed = element.GetDoubleAttribute("maxSpeed");
    angularMaxSpeed = element.GetDoubleAttribute("angularMaxSpeed");
    rotateObject = element.GetBoolAttribute("rotateObject");
    angleOffset = element.GetDoubleAttribute("angleOffset");
    extraBorder = element.GetDoubleAttribute("extraBorder");
    {
        int value = element.GetIntAttribute("cellWidth", 0);
        if (value > 0) cellWidth = value;
    }
    {
        int value = element.GetIntAttribute("cellHeight", 0);
        if (value > 0) cellHeight = value;
    }
}

#if defined(GD_IDE_ONLY)
void PathfindingBehavior::SerializeTo(gd::SerializerElement & element) const
{
    element.SetAttribute("allowDiagonals", allowDiagonals);
    element.SetAttribute("acceleration", acceleration);
    element.SetAttribute("maxSpeed", maxSpeed);
    element.SetAttribute("angularMaxSpeed", angularMaxSpeed);
    element.SetAttribute("rotateObject", rotateObject);
    element.SetAttribute("angleOffset", angleOffset);
    element.SetAttribute("cellWidth", (int)cellWidth);
    element.SetAttribute("cellHeight", (int)cellHeight);
    element.SetAttribute("extraBorder", extraBorder);
}

std::map<gd::String, gd::PropertyDescriptor> PathfindingBehavior::GetProperties(gd::Project & project) const
{
    std::map<gd::String, gd::PropertyDescriptor> properties;

    properties[_("Allows diagonals")].SetValue(allowDiagonals ? "true" : "false").SetType("Boolean");
    properties[_("Acceleration")].SetValue(gd::String::From(acceleration));
    properties[_("Max. speed")].SetValue(gd::String::From(maxSpeed));
    properties[_("Rotate speed")].SetValue(gd::String::From(angularMaxSpeed));
    properties[_("Rotate object")].SetValue(rotateObject ? "true" : "false").SetType("Boolean");
    properties[_("Angle offset")].SetValue(gd::String::From(angleOffset));
    properties[_("Virtual cell width")].SetValue(gd::String::From(cellWidth));
    properties[_("Virtual cell height")].SetValue(gd::String::From(cellHeight));
    properties[_("Extra border size")].SetValue(gd::String::From(extraBorder));

    return properties;
}

bool PathfindingBehavior::UpdateProperty(const gd::String & name, const gd::String & value, gd::Project & project)
{
    if ( name == _("Allows diagonals") ) {
        allowDiagonals = (value != "0");
        return true;
    }
    if ( name == _("Rotate object") ) {
        rotateObject = (value != "0");
        return true;
    }
    if ( name == _("Extra border size") ) {
        extraBorder = value.To<float>();
        return true;
    }

    if ( value.To<float>() < 0 ) return false;

    if ( name == _("Acceleration") )
        acceleration = value.To<float>();
    else if ( name == _("Max. speed") )
        maxSpeed = value.To<float>();
    else if ( name == _("Rotate speed") )
        angularMaxSpeed = value.To<float>();
    else if ( name == _("Angle offset") )
        angleOffset = value.To<float>();
    else if ( name == _("Virtual cell width") )
        cellWidth = value.To<unsigned int>();
    else if ( name == _("Virtual cell height") )
        cellHeight = value.To<unsigned int>();
    else
        return false;

    return true;
}

#endif