Проблемы в реализации А* алгоритма поиска пути

[sdfteh]

использую библиотеку SFML только для окна
пытался сделать алгоритм поиска пути от одной до другой точки(левая и правая кнопка мыши) окно работает но при постановке 2-ой точки приложение зависает(вывод 0 в консоли для примера)
ссылка на dropbox (полный проект)
https://www.dropbox.com/s/7bp8859hyp...nding.zip?dl=0


прикрепляю алгоритм которому придерживался

Node.h

#pragma once
#include <math.h>
#include "SFML\System.hpp"
 
class Node
{
public:
    bool walkable;
    int gridX, gridY;
    int id;
    Node *parent;
    float gCost;
    float hCost;
 
    Node() : parent(0) {}
    Node(int _id, int x, int y,  Node* _parent = 0)
    {
        walkable = true;
        gridX = x;
        gridY = y;
        parent = _parent;
        id = _id;
        gCost = 0;
        hCost = 0;
    };
    float fCost() { return gCost + hCost; }
};

Grid.h

#pragma once
#include "Node.h"
#include<vector>
 
class Grid
{
public:
    int gridSizeX, gridSizeY;
    float nodeRadius;
    std::vector<std::vector<Node>> grid;
    std::vector<Node> tempGrid;
    Grid() {};
    ~Grid() {};
    void UpdateGrid()
    {
        for (int x = 0; x < gridSizeX; x++)
        {
            for (int y = 0; y < gridSizeY; y++)
            {
                Node node;
                node.id = y * gridSizeX + x;
                tempGrid.push_back(node);
            }
            grid.push_back(tempGrid);
            tempGrid.clear();
        }
        grid.push_back(tempGrid);
    };
    Node NodeFromWorldPoint(sf::Vector2f worldPosition)
    {
        float nodeDiameter = nodeRadius * 2;
        int x = worldPosition.x / nodeDiameter;
        int y = worldPosition.y / nodeDiameter;
        return grid[x][y];
    }
 
};

PathFinding.h

#pragma once
#include "Grid.h"
#include <vector>
 
class PathFinding
{
public:
    std::vector<Node*> openSet;
    std::vector<Node*> closedSet;
    std::vector<Node*> path;
    Node *startNode;
    Node* targetNode;
    bool intialized;
    Grid grid;
    PathFinding(int _gridSizeX, int _gridSizeY, float _nodeRadius);
    ~PathFinding();
 
    void FindPath(sf::Vector2f startPos, sf::Vector2f targetPos);
    void PathOpened(int x, int y, Node *parent);
    void SetStartAndGoal(Node start, Node goal);
    void RetracePath(Node* startNode, Node* endNode);
    int GetDistance(Node* nodeA, Node* nodeB);
};

[sdfteh]

PathFinding.cpp

#include "PathFinding.h"
  
 
PathFinding::PathFinding(int _gridSizeX, int _gridSizeY, float _nodeRadius)
{
    grid.gridSizeX = _gridSizeX;
    grid.gridSizeY = _gridSizeY;
    grid.nodeRadius = _nodeRadius;
    grid.UpdateGrid();
    intialized = false;
}
 
PathFinding::~PathFinding()
{
    delete(startNode);
    delete(targetNode);
    for (int i = 0; i < openSet.size(); i++)
    {
        delete openSet[i];
    }
    openSet.clear();
 
    for (int i = 0; i < closedSet.size(); i++)
    {
        delete closedSet[i];
    }
    closedSet.clear();
    for (int i = 0; i < path.size(); i++)
    {
        delete path[i];
    }
    path.clear();
}
 
void PathFinding::FindPath(sf::Vector2f startPos, sf::Vector2f targetPos)
{
    if (!intialized)
    {
        for (int i = 0; i < openSet.size(); i++)
        {
            delete openSet[i];
        }
        openSet.clear();
 
        for (int i = 0; i < closedSet.size(); i++)
        {
            delete closedSet[i];
        }
        closedSet.clear();
        for (int i = 0; i < path.size(); i++)
        {
            delete path[i];
        }
        path.clear();
 
        Node start = grid.NodeFromWorldPoint(startPos);
        Node target = grid.NodeFromWorldPoint(targetPos);
 
        SetStartAndGoal(start, target);
        intialized = true;
    }
    if (intialized)
    {
        while (openSet.size() > 0)
        {
            int index = 0;
            Node* node = openSet[0];
            for (int i = 1; i < openSet.size(); i++)
            {
                if (openSet[i]->fCost() < node->fCost() || openSet[i]->fCost() == node->fCost())
                {
                    if (openSet[i]->hCost < node->hCost)
                    {
                        node = openSet[i];
                        index = i;
                    }
                }
            }
 
            openSet.erase(openSet.begin() + index);
            closedSet.push_back(node);
 
            if (node->id == targetNode->id)
            {
                RetracePath(startNode, targetNode);
                return;
            }
 
            for (int x = -1; x <= 1; x++)
            {
                for (int y = -1; y <= 1; y++)
                {
                    if (x == 0 && y == 0)
                        continue;
 
                    int checkX = node->gridX + x;
                    int checkY = node->gridY + y;
 
                    if (checkX >= 0 && checkX < grid.gridSizeX && checkY >= 0 && checkY < grid.gridSizeY)
                    {
                        PathOpened(checkX, checkY, node);
                    }
                }
            }
        }
    }
}
 
void PathFinding::SetStartAndGoal(Node start, Node goal)
{
    startNode = new Node(start.id, start.gridX, start.gridY, NULL);
    targetNode = new Node(goal.id, goal.gridX, goal.gridY, &goal);
 
    startNode->gCost = 0;
    startNode->hCost = GetDistance(startNode, targetNode);
    startNode->parent = 0;
 
    openSet.push_back(startNode);
}
 
void PathFinding::PathOpened(int x, int y, Node *parent)
{
    int _id = y* grid.gridSizeX + x;
    for (int i = 0; i < closedSet.size(); i++)
    {
        if (_id == closedSet[i]->id)
        {
            return;
        }
    }
 
    Node* neighbour = new Node(y* grid.gridSizeX + x, x, y, parent);
 
    int newCostToNeighbour = parent->gCost + GetDistance(parent, neighbour);
    if (newCostToNeighbour < neighbour->gCost || !(std::find(openSet.begin(), openSet.end(), neighbour) != openSet.end()))
    {
        neighbour->gCost = newCostToNeighbour;
        neighbour->hCost = GetDistance(neighbour, targetNode);
 
        if (!(std::find(openSet.begin(), openSet.end(), neighbour) != openSet.end()))
            openSet.push_back(neighbour);
    }
 
    if (!(std::find(openSet.begin(), openSet.end(), neighbour) != openSet.end()))
        openSet.push_back(neighbour);
 
    openSet.push_back(neighbour);
}
 
void PathFinding::RetracePath(Node* startNode, Node* endNode)
{
    Node* currentNode = endNode;
 
    while (currentNode->id != startNode->id)
    {
        path.push_back(currentNode);
        currentNode = currentNode->parent;
    }
    std::reverse(path.begin(), path.end());
}
 
int PathFinding::GetDistance(Node* nodeA, Node* nodeB)
{
    int dstX = fabs(nodeA->gridX - nodeB->gridX);
    int dstY = fabs(nodeA->gridY - nodeB->gridY);
 
    if (dstX > dstY)
        return 14 * dstY + 10 * (dstX - dstY);
    return 14 * dstX + 10 * (dstY - dstX);
}

main.cpp

#include"SFML\Graphics.hpp"
#include<iostream>
#include<vector>
#include"PathFinding.h"
#include "Grid.h"
 
int main()
{
    sf::Vector2i gridSize(32,32);
    int nodeRadius = 8;
    int nodeDiameter = nodeRadius * 2;
    PathFinding pathfinding(gridSize.x, gridSize.y, nodeRadius);
    bool findPath = false;
 
    sf::Texture tileTexture;
    sf::Sprite tiles;
 
    std::vector<std::vector<sf::Vector2i>> map;
    std::vector<sf::Vector2i> tempMap;
 
    sf::Vector2f start(-1,-1), target(-1,-1);
 
    tileTexture.loadFromFile("tiles.png");
    tiles.setTexture(tileTexture);
    for(int i = 0; i < gridSize.x; i++)
    {
        for (int j = 0; j < gridSize.y; j++)
        {
            tempMap.push_back(sf::Vector2i(0, 0));
        }
        map.push_back(tempMap);
        tempMap.clear();
    }
    map.push_back(tempMap);
 
    sf::RenderWindow window(sf::VideoMode(512, 512, 32), "Pathfind");
 
    while (window.isOpen())
    {
        sf::Event Event;
        while (window.pollEvent(Event))
        {
            sf::Vector2i mousePos = sf::Mouse::getPosition(window);
            switch (Event.type)
            {
            case sf::Event::Closed:
                window.close();
                break;
            }
 
            if (sf::Mouse::isButtonPressed(sf::Mouse::Left))
            {
                map[mousePos.y / nodeDiameter][mousePos.x / nodeDiameter] = sf::Vector2i(1, 0);
                start.x = mousePos.x;
                start.y = mousePos.y;
            }
            else if (sf::Mouse::isButtonPressed(sf::Mouse::Right))
            {
                map[mousePos.y / nodeDiameter][mousePos.x / nodeDiameter] = sf::Vector2i(1, 1);
                target.x = mousePos.x;
                target.y = mousePos.y;
            }
        }
        window.clear(sf::Color(0,240,255));
 
        std::cout << '0';
 
        if (start.x != -1 && target.x != -1 && start.y != -1 && target.y != -1)
        {
            pathfinding.FindPath(start, target);
            findPath = true;
        }
 
        for (int i = 0; i < map.size(); i++)
        {
            for (int j = 0; j < map[i].size(); j++)
            {
                if (map[i][j].x != -1 && map[i][j].y != -1)
                {
                    tiles.setPosition(j * 16, i * 16);
                    tiles.setTextureRect(sf::IntRect(map[i][j].x * 32, map[i][j].y * 32, nodeDiameter, nodeDiameter));
                    if (findPath)
                    {
                        for (int k = 0; k < pathfinding.path.size(); k++)
                        {
                            bool is_print=false;
                            if (pathfinding.path[k]->gridX == map[i][j].x || pathfinding.path[k]->gridY == map[i][j].y)
                            {
                                is_print=true;
                            }
                            if (is_print)
                            {
                                tiles.setTextureRect(sf::IntRect(0, 32, nodeDiameter, nodeDiameter));
                            }
                        }
                    }
                    window.draw(tiles);
                }
            }
        }
 
        window.display();
    }
    return 0;
}

tiles.png просто четыре квадрата 16*16

Большое спасибо за помощь

[Croessmah]

Лучше выложите архивом (только не rar),
проект, готовый для сборки и запуска.

[sdfteh]

Папка проекта в сжатом виде превышает лимит установленный этим сайтом
Вот ссылка на него(zip) через dropbox
https://www.dropbox.com/s/7bp8859hyp...nding.zip?dl=0

Если вы подразумевали файл - решение или файл с расширением vcproject
то я их приложил к этому сообщению