0
\$\begingroup\$

I'm trying to use A* and visualised it, but when i add a heap optimization it start looking in incredeble ways sometimes normal(better then next to be sure), somtimes he looks on few columns. Example with heap

Normal working without heap

Example without heap

Code:

Pathfinding.h

#pragma once
#include "Node.h"
#include<iostream>
#include "Heap.h"


class Pathfinding
{
public:
    std::vector<Node*> openSet;
    std::vector<Node*> closedSet;
    std::vector<Node*> path;
    std::vector<std::vector<Node*>> grid;
    Node* startNode;
    Node* targetNode;
    bool intialized = false;
    int cols;
    int rows;
    bool find = false;
    Pathfinding(int _cols, int _rows, std::vector<std::vector<Node*>> _grid);
    ~Pathfinding();

    void FindPath(Node* start, Node* target);
    void SetStartAndGoal(Node* start, Node* goal);
    int GetDistance(Node* nodeA, Node* nodeB);
};

Pathfinding.cpp

#include "Pathfinding.h"



Pathfinding::Pathfinding(int _cols, int _rows, std::vector<std::vector<Node*>> _grid)
{
cols = _cols;
rows = _rows;
grid = _grid;
}


Pathfinding::~Pathfinding()
{
delete startNode;
delete targetNode;
openSet.clear();
closedSet.clear();
path.clear();
}

void Pathfinding::FindPath(Node* start, Node* target)
{
if (!intialized)
{

    openSet.clear();
    closedSet.clear();
    path.clear();

    std::make_heap(openSet.begin(), openSet.end());

    SetStartAndGoal(start, target);

    intialized = true;
}
if (intialized)
{
    if (!find)
    {
        if (openSet.size() > 0)  
        {
            Node* current = openSet.front();

            std::pop_heap(openSet.begin(), openSet.end());
            openSet.pop_back();

            if (current == targetNode)
            {
                Node* temp = current;
                path.push_back(temp);
                while (temp->parent)
                {
                    path.push_back(temp->parent);
                    temp = temp->parent;
                }
                std::cout << "DONE!";
                find = true;
            }

            closedSet.push_back(current);

            for (int i = -1; i <= 1; i++)
            {
                for (int j = -1; j <= 1; j++)
                {
                    if (i == 0 && j == 0)
                        continue;

                    int checkX = current->x + i;
                    int checkY = current->y + j;

                    if (checkX >= 0 && checkX < cols && checkY >= 0 && checkY < rows)
                    {
                        Node* neighbor = grid[checkX][checkY];
                        bool inClosed = false;
                        for (int k = 0; k < closedSet.size(); k++)
                        {
                            if (neighbor == closedSet[k])
                            {
                                inClosed = true;
                            }
                        }
                        if (!inClosed && neighbor->walkable)
                        {
                            int tempG = current->g + GetDistance(current, neighbor);

                            bool inOpen = false;
                            for (int k = 0; k < openSet.size(); k++)
                            {
                                if (neighbor == openSet[k])
                                {
                                    inOpen = true;
                                }
                            }
                            if (inOpen)
                            {
                                if (tempG < neighbor->g)
                                {
                                    neighbor->g = tempG;
                                    neighbor->parent = current;
                                }
                            }
                            else
                            {
                                neighbor->g = tempG;
                                openSet.push_back(neighbor);
                                std::push_heap(openSet.begin(), openSet.end());
                                neighbor->parent = current;
                            }
                            neighbor->h = GetDistance(neighbor, targetNode);
                            neighbor->f = neighbor->g + neighbor->h;

                        }
                    }
                }
            }
        }
        else
        {
            std::cout << "NO SOLUTION";
            find = true;
        }
    }
}
}

void Pathfinding::SetStartAndGoal(Node* start, Node* goal)
{
startNode = new Node(start->x, start->y);
targetNode = new Node(goal->x, goal->y);

startNode = start;
targetNode = goal;

startNode->g = 0;
startNode->h = GetDistance(startNode, targetNode);
startNode->parent = 0;

openSet.push_back(startNode);
std::push_heap(openSet.begin(), openSet.end());
}

int Pathfinding::GetDistance(Node* nodeA, Node* nodeB)
{
int dstX = fabs(nodeA->x - nodeB->x);
int dstY = fabs(nodeA->y - nodeB->y);

if (dstX > dstY)
    return 14 * dstY + 10 * (dstX - dstY);
return 14 * dstX + 10 * (dstY - dstX);
}

bool cmp(const Node &a, const Node &b)
{
if (a.f == b.f)
    if (a.h > b.h)
        return true;
    else
        return false;
else if (a.f > b.f)
    return true;
else
    return false;
}

Node.h

#pragma once
#include <SFML/Graphics.hpp>

class Node
{
public:
int x, y;
float f = 0;
float g = 0;
float h = 0;
bool walkable = true;
Node* parent = NULL;
sf::Color col = sf::Color::White;
Node(int i, int j);
~Node();
};

Node.cpp

#include "Node.h"


Node::Node(int i, int j)
{
x = i;
y = j;
}


Node::~Node()
{}

main

#include <SFML/Graphics.hpp>
#include "Node.h"
#include <windows.h>
#include <time.h>  
#include "Pathfinding.h"



int cols = 50;
int rows = 50;
int width = 800;
int height = 800;
int w, h;

std::vector<std::vector<Node*>> grid;
std::vector<Node*> tempGrid;

std::vector<Node*> openSet;
std::vector<Node*> closedSet;
std::vector<Node*> path;
Node* start;
Node* end;

int main()
{
//srand(time(NULL));

sf::RenderWindow window(sf::VideoMode(width, height), "A*");

w = width / cols;
h = height / rows;

for (int i = 0; i < cols; i++)
{
    for (int j = 0; j < rows; j++)
    {
        Node* node = new Node(i, j);
        float r = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
        if (r < 0.3 && !(i == 0 && j == 0 || i == cols - 1 && j == rows - 1))
            node->walkable = false;
        tempGrid.push_back(node);
    }
    grid.push_back(tempGrid);
    tempGrid.clear();
}

Pathfinding pathfind(cols, rows, grid);

while (window.isOpen())
{
    sf::Event event;
    while (window.pollEvent(event))
    {
        if (event.type == sf::Event::Closed)
            window.close();
    }

    //algorithm

    pathfind.FindPath(grid[0][0], grid[cols - 1][rows - 1]);
    openSet = pathfind.openSet;
    closedSet = pathfind.closedSet;
    path = pathfind.path;

    //draw

    window.clear(sf::Color::Black);

    for (int i = 0; i < closedSet.size(); i++)
    {
        closedSet[i]->col = sf::Color::Red;
    }

    for (int i = 0; i < openSet.size(); i++)
    {
        openSet[i]->col = sf::Color::Green;
    }

    for (int i = 0; i < path.size(); i++)
    {
        path[i]->col = sf::Color::Blue;
    }

    for (int i = 0; i < cols; i++)
    {
        for (int j = 0; j < rows; j++)
        {
            sf::RectangleShape rectangle(sf::Vector2f(w, h));
            rectangle.setPosition(i*w, j*h);
            if (!grid[i][j]->walkable)
                rectangle.setFillColor(sf::Color::Black);
            else
                rectangle.setFillColor(grid[i][j]->col);
            rectangle.setOutlineThickness(-1);
            rectangle.setOutlineColor(sf::Color::Black);
            window.draw(rectangle);
        }
    }

    window.display();
}


for (int i = 0; i < grid.size(); i++)
{
    grid[i].clear();
}
grid.clear();


return 0;

}

What I doing wrong?

Thanks for help!

\$\endgroup\$
2
  • 1
    \$\begingroup\$ Hint: You can take a screenshot of just the active window with Alt+Print. \$\endgroup\$ Commented Jan 19, 2017 at 11:40
  • \$\begingroup\$ @Philipp I learn something new from you just about every day. All though it can be applicable to games, I was thinking this post may be better over on Code Review SE. \$\endgroup\$ Commented Jan 19, 2017 at 12:40

1 Answer 1

Reset to default
1
\$\begingroup\$ 
if (tempG < neighbor->g)
{
    neighbor->g = tempG;
    neighbor->parent = current;
}

After this the heap needs to be updated so the neighbor properly bubbles up to preserve the heap property.

It would be much easier if there was a std::change_element_heap(begin, end, it); which assumes that except for the element at it, the heap invariant holds and then fixes the heap. It's possible to implement that yourself. It still requires that you find where the Node is in the heap before you can update it.

\$\endgroup\$
8
  • \$\begingroup\$ find that i dont use cmp function to compare in heap also i rearrange this fragment to if (inOpen) { if (tempG < neighbor->g) { neighbor->g = tempG; neighbor->h = GetDistance(neighbor, targetNode); neighbor->f = neighbor->g + neighbor->h; neighbor->parent = current; std::sort_heap(openSet.begin(), openSet.end(), cmp); } } but now debug says that this is invalid heap \$\endgroup\$ Commented Jan 19, 2017 at 12:07
  • \$\begingroup\$ @ibvfteh s/sort_heap/make_heap/ sort heap doesn't do what you want, it instead does repeated pop_heap until the heap is empty and leaves the range sorted. \$\endgroup\$ Commented Jan 19, 2017 at 12:13
  • \$\begingroup\$ actually i tried to use make_heap and it works nicely but if you say that it don't do what i want then maybe add something like openSet.erase(std::remove(openSet.begin(), openSet.end(), neghibor), openSet.end()); std::pop_heap(openSet.begin(), openSet.end(), cmp); change parameters openSet.push_back(neighbor); std::push_heap(openSet.begin(), openSet.end(), cmp); ? but i not sure that after openSet would be a valid heap \$\endgroup\$ Commented Jan 19, 2017 at 12:30
  • \$\begingroup\$ make_heap will fix the heap but it's less efficient than if you could specify which elements were violating the heap invariant. \$\endgroup\$ Commented Jan 19, 2017 at 12:40
  • 1
    \$\begingroup\$ but how can i do that? \$\endgroup\$ Commented Jan 19, 2017 at 13:31

You must log in to answer this question.

Start asking to get answers

Find the answer to your question by asking.

Ask question

Explore related questions

See similar questions with these tags.