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
Большое спасибо за помощь