Pathfinding - Podążanie wyznaczoną ścieżką

void loadTheTarget() {
    target.x = base.x + rand() % 200 - 100;
    target.y = base.y + rand() % 200 - 100;
    cout << "Load the target: [" << target.x << "," << target.y << "]\n";
}

void loadThePath() {
   
    Point goal = Point( target.x, target.y );
   
    path = aStar( this, goal );
    //path.push_back(goal);
   
    cout << "monster: " << name << "\n";
    cout << "position: [" << position.x << "," << position.y << "]\n";
    cout << "path(" << path.size() << ") : ";
   
    for( auto & p: path )
         cout << "[" << p.x << "," << p.y << "] ";
   
    cout << "\n";
}

void followThePath( float dt ) {
   
    float dist = 15.0f * 4.0f * dt; // distance to move
   
    if( position.x == target.x && position.y == target.y ) {
        loadTheTarget();
        loadThePath();
    }
    else if( position.x != target.x && fabs( target.x - position.x ) < dist ) {
        position.x = target.x;
    }
    else if( position.y != target.y && fabs( target.y - position.y ) < dist ) {
        position.y = target.y;
    }
    else if( position.x == path[ 0 ].x && position.y == path[ 0 ].y ) {
        path.erase( path.begin() );
    }
    else if( position.x != path[ 0 ].x && fabs( path[ 0 ].x - position.x ) < dist ) {
        position.x = path[ 0 ].x;
    }
    else if( position.y != path[ 0 ].y && fabs( path[ 0 ].y - position.y ) < dist ) {
        position.y = path[ 0 ].y;
    }
    else if( path[ 0 ].x < position.x ) {
        position.x -= dist;
    }
    else if( path[ 0 ].x > position.x ) {
        position.x += dist;
    }
    else if( path[ 0 ].y < position.y ) {
        position.y -= dist;
    }
    else if( path[ 0 ].y > position.y ) {
        position.y += dist;
    }
   
}

#ifndef Pathfinding_hpp
#define Pathfinding_hpp

#include <iostream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <algorithm>

float stepsize = 32.0f;

class Point {
public:
    float x, y;
    Point()
        : x( 0 )
        , y( 0 )
    { }
    Point( float x, float y )
        : x( x )
        , y( y )
    { }
   
    bool operator ==( const Point & other ) const {
        return x == other.x && y == other.y;
    }
   
    bool operator !=( const Point & other ) const {
        return x != other.x || y != other.y;
    }
   
    bool operator <( const Point & other ) const {
        return std::tie( x, y ) < std::tie( other.x, other.y );
    }
};

namespace std {
    template < >
    struct hash < Point > {
        size_t operator()( const Point & p ) const {
            return hash < float >()( p.x ) ^ hash < float >()( p.y );
        }
    };
}

float heuristic( const Point & a, const Point & b ) {
    return abs( a.x - b.x ) + abs( a.y - b.y );
}

std::vector < Point > getNeighbors( GameObject * object, const Point & p ) {
    std::vector < Point > neighbors;
   
    //if(!collisionPrediction(object, -stepsize, 0))
    neighbors.emplace_back( p.x - stepsize, p.y );
   
    //if (!collisionPrediction(object, stepsize, 0))
    neighbors.emplace_back( p.x + stepsize, p.y );
   
    //if (!collisionPrediction(object, 0, -stepsize))
    neighbors.emplace_back( p.x, p.y - stepsize );
   
    //if (!collisionPrediction(object, 0, stepsize))
    neighbors.emplace_back( p.x, p.y + stepsize );
   
    //if (!collisionPrediction(object, -stepsize, -stepsize))
    neighbors.emplace_back( p.x - stepsize, p.y - stepsize );
   
    //if (!collisionPrediction(object, -stepsize, stepsize))
    neighbors.emplace_back( p.x - stepsize, p.y + stepsize );
   
    //if (!collisionPrediction(object, stepsize, -stepsize))
    neighbors.emplace_back( p.x + stepsize, p.y - stepsize );
   
    //if (!collisionPrediction(object, stepsize, stepsize))
    neighbors.emplace_back( p.x + stepsize, p.y + stepsize );
   
    return neighbors;
}

std::vector < Point > aStar( GameObject * object, const Point & goal ) {
    const Point & start = Point( object->position.x, object->position.y );
    std::priority_queue < std::pair < float, Point >, std::vector < std::pair < float, Point >>, std::greater < >> openSet;
    std::unordered_map < Point, Point > cameFrom;
    std::unordered_map < Point, int > costSoFar;
   
    openSet.emplace( 0, start );
    cameFrom[ start ] = start;
    costSoFar[ start ] = 0;
   
    while( !openSet.empty() ) {
        Point current = openSet.top().second;
        openSet.pop();
       
        if( current == goal ) {
            std::vector < Point > path;
            while( current != start ) {
                path.push_back( current );
                current = cameFrom[ current ];
            }
            path.push_back( start );
            std::reverse( path.begin(), path.end() );
            return path;
        }
        else if( fabs( current.x - goal.x ) < stepsize && fabs( current.y - goal.y ) < stepsize )
        {
            Point next = Point( goal.x, goal.y );
            float newCost = costSoFar[ current ] + 1;
            if( costSoFar.find( next ) == costSoFar.end() || newCost < costSoFar[ next ] ) {
                costSoFar[ next ] = newCost;
                float priority = newCost + heuristic( next, goal );
                openSet.emplace( priority, next );
                cameFrom[ next ] = current;
            }
        }
        else {
            for( const Point & next: getNeighbors( object, current ) ) {
                float newCost = costSoFar[ current ] + 1;
                if( costSoFar.find( next ) == costSoFar.end() || newCost < costSoFar[ next ] ) {
                    costSoFar[ next ] = newCost;
                    float priority = newCost + heuristic( next, goal );
                    openSet.emplace( priority, next );
                    cameFrom[ next ] = current;
                }
            }
        }
       
       
    }
   
    return { }; // No path found
}
#endif