std::vector<Node*> AStar::GeneratePath(Node* start, Node* goal)
{
    int c = 0;
    std::vector<Node*> openSet;
    std::vector<Node*> closedSet;

    start->g = 0;

    openSet.push_back(start);

    while (openSet.size() > 0)
    {
        Node* currentNode = openSet.at(0);
        for (int i = 0; i < openSet.size(); ++i)
        {
            if (openSet.at(i)->f < currentNode->f || openSet.at(i)->f == currentNode->f && openSet.at(i)->h < currentNode->h)
            {
                currentNode = openSet.at(i);
            }
        }

        Remove(&openSet, currentNode);
        closedSet.push_back(currentNode);

        if (currentNode->x == goal->x && currentNode->y == goal->y)
        {
            goal->parent = currentNode->parent;
            break;
        }

        for (Node* neighbour : GetNeighbours(currentNode))
        {
            if(neighbour != nullptr)
            {
                if (Contains(&closedSet, neighbour) || neighbour->e == 0) continue;

                int newMovementCostToNeighbour = currentNode->g + GetDistance(currentNode, neighbour) + neighbour->penalty;
                if (newMovementCostToNeighbour < neighbour->g || !Contains(&openSet, neighbour))
                {
                    neighbour->g = newMovementCostToNeighbour;
                    neighbour->h = GetDistance(neighbour, goal);
                    neighbour->f = neighbour->g + neighbour->h;
                    neighbour->parent = currentNode;

                    if (!Contains(&closedSet, neighbour)) openSet.push_back(neighbour);
                    else openSet;
                }
            }
        }
    }
 
    std::vector<Node*> nodes;
    Node* current = goal;
    while (current != nullptr)
    {
        nodes.push_back(current);
        current = current->parent;
    }
 
    reverse(nodes.begin(), nodes.end());

    return nodes;
}

int AStar::GetDistance(Node* nodeA, Node* nodeB)  
{
    int distX = abs(nodeA->x - nodeB->x);
    int distY = abs(nodeA->y - nodeB->y);
 
    if (distX > distY) return distY * (distX - distY);
    return distX + (distY - distX);
}

std::vector<Node*> AStar::GetNeighbours(Node* _pNode)
{
    std::vector<Node*> neighbours;
    for (int x = -1; x <= 1; ++x)
    {
        if (_pNode == nullptr)
        {
            break;
        }
        for (int y = -1; y <= 1; ++y)
        {
            if (x == 0 && y == 0) continue;

            int checkX = _pNode->x + x;
            int checkY = _pNode->y + y;

            if (checkX >= 0 && checkX < 512 && checkY >= 0 && checkY < 512)
            {
                neighbours.push_back(GetNodeAt(checkX, checkY));
            }
        }
    }
    return neighbours;
}

bool AStar::Contains(std::vector<Node*>* _pVector, Node* _pNode)
{
    if(_pNode == nullptr)
    {
        return false;
    }
    if (find(_pVector->begin(), _pVector->end(), _pNode) != _pVector->end())
    {
        return true;
    }
    return false;
}

void AStar::Remove(std::vector<Node*>* _pVector, Node* _pNode)
{
    for (std::vector<Node*>::iterator iter = _pVector->begin(); iter != _pVector->end(); ++iter)
    {
        if (*iter == _pNode)
        {
            _pVector->erase(iter);
            break;
        }
    }
}

Node* AStar::GetNodeAt(int _x, int _y)
{
    for (Node* node : EntityManager::GetInstance()->GetAStar()->GetNodeMap())
    {
        if (node->x == _x && node->y == _y)
        {
            return node;
        }
    }
    return nullptr;
}

 #include "AStar.h"
#include "Node.h"
#include "EntityManager.h"
#include <algorithm>
#include <iostream>
#include <unordered_set>
#include <queue>
 
using namespace Dotmk_AI;
using namespace Dotmk_Managers;
 
AStar::AStar()
{
 
}
 
std::vector<Node*> AStar::GeneratePath(Node* start, Node* goal)
{
    std::vector<std::vector<Node*>>* pNodeMap = EntityManager::GetAStar()->GetNodeMap();
 
    std::vector<Node*> openSet;
    std::unordered_set<Node*> closedSet;
 
    make_heap(openSet.begin(), openSet.end());
 
    start->g = 0;
 
    openSet.push_back(start);
    push_heap(openSet.begin(), openSet.end());
    while (openSet.size() > 0)
    {
        Node* currentNode = openSet[0];
        for (int i = 0; i < openSet.size(); ++i)
        {
            if (openSet[0]->f < currentNode->f || openSet[0]->f == currentNode->f && openSet[0]->h < currentNode->h)
            {
                currentNode = openSet[i];
            }
        }
 
        Remove(&openSet, currentNode);
        closedSet.insert(currentNode);
 
        if (currentNode->x == goal->x && currentNode->y == goal->y)
        {
            goal->parent = currentNode->parent;
            break;
        }
 
        for (Node* neighbour : GetNeighbours(currentNode, pNodeMap))
        {
            if (neighbour != nullptr)
            {
                if (Contains(closedSet, neighbour) || neighbour->e == 0) continue;
 
                int newMovementCostToNeighbour = currentNode->g + GetDistance(currentNode, neighbour);
                if (newMovementCostToNeighbour < neighbour->g || !Contains(openSet, neighbour))
                {
                    neighbour->g = newMovementCostToNeighbour;
                    neighbour->h = GetDistance(neighbour, goal);
                    neighbour->f = neighbour->g + neighbour->h;
                    neighbour->parent = currentNode;
       
                    if (!Contains(openSet, neighbour)) openSet.push_back(neighbour);
                }
            }
        }
    }
    std::vector<Node*> nodes;
    Node* current = goal;
    while (current != nullptr)
    {
        nodes.push_back(current);
        current = current->parent;
    }
    reverse(nodes.begin(), nodes.end());
    nodes.pop_back();
    std::cout << nodes.size() << std::endl;
 
    return nodes;
}
 
int AStar::GetDistance(Node* nodeA, Node* nodeB)  
{
    int distX = abs(nodeA->x - nodeB->x);
    int distY = abs(nodeA->y - nodeB->y);
  
    return 1 * (distX * distX + distY * distY);
}
 
std::vector<Node*> AStar::GetNeighbours(Node* _pNode, std::vector<std::vector<Node*>>* _pNodeMap)
{
    std::vector<Node*> neighbours;
    neighbours.reserve(8);
    for (int x = -1; x <= 1; ++x)
    {
        for (int y = -1; y <= 1; ++y)
        {
            if (x == 0 && y == 0) continue;
 
            int checkX = _pNode->x + x;
            int checkY = _pNode->y + y;
 
            if (checkX >= 0 && checkX < 64 && checkY >= 0 && checkY < 64)
            {
                neighbours.push_back((*_pNodeMap)[checkX][checkY]);
            }
        }
    }
    return neighbours;
}
 
bool AStar::Contains(std::vector<Node*> _pVector, Node* _pNode)
{
    return find(_pVector.begin(), _pVector.end(), _pNode) != _pVector.end();
}
 
bool AStar::Contains(std::unordered_set<Node*> _pVector, Node* _pNode)
{
    return find(_pVector.begin(), _pVector.end(), _pNode) != _pVector.end();
}
 
void AStar::Remove(std::vector<Node*>* _pVector, Node* _pNode)
{
    int pos = find(_pVector->begin(), _pVector->end(), _pNode) - _pVector->begin();
    auto it = _pVector->begin() + pos;
    rotate(it, it + 1, _pVector->end());
    _pVector->pop_back();
}

EDIT: Setting it on Release mode will make the pathfinding ALMOST instant, but still noticable. I've taken much of your guys advice and improved the code.

My grid is only 64*64.