A* Pathfinder in C++ - c++

Question

I am trying to write an A* pathfinder to learn C++ and am struggling with a specific bug.
These are the features/assumptions of the program:
The path contains "walls" which cannot be passed
All paths cost the same value (1)
Diagonal movement is allowed
Only one goal
Here is my code:
#include <algorithm>
#include <iostream>
#include <vector>
#include <tuple>
#include <cmath>
#include <set>
using Graph = std::vector<std::vector<int>>;
using Position = std::tuple<int, int>;
class Node;
using Nodes = std::vector<Node>;
constexpr int WALL_BLOCK = 1;
constexpr int USER_BLOCK = 3;
constexpr int GOAL_BLOCK = 2;
class Node {
private:
int row;
int column;
Node* parent = nullptr;
int f = 0;
int g = 0;
int h = 0;
public:
Node() : row(-1), column(-1) {}
Node(int row, int column) : row(row), column(column) {}
Node(int row, int column, Node *parent) : row(row), column(column) {
if (this->parent != nullptr && *this->parent == *this) {
throw "Node cannot be parented to itself";
}
this->parent = parent;
}
~Node() {}
Node* get_parent() const { return this->parent; }
int get_f() const { return this->f; }
int get_g() const { return this->g; }
int get_h() const { return this->h; }
Position get_position() const { return std::make_tuple(row, column); }
void set_f(int f) { this->f = f; }
void set_g(int g) { this->g = g; }
void set_h(int h) { this->h = h; }
bool operator==(Node const &node) const {
return this->row == node.row && this->column == node.column;
}
bool operator<(Node const &node) const {
return this->row < node.row && this->column < node.column;
}
friend std::ostream& operator<<(std::ostream& os, Node const &node);
};
std::ostream& operator<<(std::ostream& os, Node const &node) {
auto row = node.row;
auto column = node.column;
os << "Node(("
<< row << ", "
<< column << "), "
<< node.get_g() << ", "
<< node.get_h() << ", "
;
Node* parent = node.get_parent();
if (parent != nullptr) {
os << parent;
} else {
os << "nullptr";
}
os << ")";
return os;
}
inline bool is_walkable(Node const &node, Graph const &graph) {
int column;
int row;
std::tie(row, column) = node.get_position();
return graph[row][column] != WALL_BLOCK;
}
Position get_first_index(Graph const &graph, int block);
Position get_end(Graph const &graph) {
return get_first_index(graph, GOAL_BLOCK);
}
Position get_first_index(Graph const &graph, int block) {
for (int row = 0, max = graph.size(); row < max; ++row) {
auto line = graph[row];
auto found = std::find(line.begin(), line.end(), block);
if (found != line.end()) {
return std::make_tuple(row, found - line.begin());
}
}
return {-1, -1};
}
inline int get_h(Node const &node, Node const &reference) {
auto node_position = node.get_position();
auto reference_position = reference.get_position();
auto node_position_row = std::get<0>(node_position);
auto node_position_column = std::get<1>(node_position);
auto reference_position_row = std::get<0>(reference_position);
auto reference_position_column = std::get<1>(reference_position);
return (
std::pow((node_position_row - reference_position_row), 2) +
std::pow((node_position_column - reference_position_column), 2)
);
}
Position get_start(Graph const &graph) {
return get_first_index(graph, USER_BLOCK);
}
Nodes get_children(Node &node, Graph const &graph) {
Nodes children;
int row;
int column;
std::tie(row, column) = node.get_position();
for (int row_offset = -1; row_offset < 2; ++row_offset) {
for (int column_offset = -1; column_offset < 2; ++column_offset) {
if (row_offset == 0 and column_offset == 0) {
// (0, 0) will always be `node`. We can't let `node` be a child
// of itself so we have to `continue` here
//
continue;
}
Graph::size_type node_row = row + row_offset;
Graph::size_type node_column = column + column_offset;
if (node_row >= graph.size()) {
continue;
}
if (node_column >= graph[node_row].size()) {
continue;
}
children.push_back({
static_cast<int>(node_row),
static_cast<int>(node_column),
&node
});
}
}
return children;
}
Nodes trace(Node const &node) {
Node* parent = node.get_parent();
Nodes path;
std::set<Node> seen;
while (parent != nullptr) {
auto parent_node = *parent;
if (std::find(seen.begin(), seen.end(), parent_node) != seen.end()) {
// If this happens, `parent` is already in `path`. To avoid
// a cyclic loop from happening, we will break out, instead.
//
break;
}
seen.insert(parent_node);
path.push_back(parent_node);
parent = parent->get_parent();
}
return path;
}
Nodes a_star(Graph const &graph, Node const &user, Node const &goal) {
Nodes open_list {user};
Nodes closed_list;
while (open_list.size() != 0) {
Node current_node = open_list[0];
unsigned int current_index = 0;
for (int index = 0, max = open_list.size(); index < max; ++index) {
auto node = open_list[index];
if (node.get_f() < current_node.get_f()) {
current_node = node;
current_index = index;
}
}
if (current_node == goal) {
auto path = trace(current_node);
std::reverse(path.begin(), path.end());
return path;
}
open_list.erase(open_list.begin() + current_index);
closed_list.push_back(current_node);
auto children = get_children(current_node, graph);
for (auto &child : children) {
if (std::find(closed_list.begin(), closed_list.end(), child) != closed_list.end()) {
continue;
}
if (!is_walkable(child, graph)) {
continue;
}
child.set_g(child.get_parent()->get_g() + 1);
child.set_h(get_h(child, goal));
child.set_f(child.get_g() + child.get_h());
bool add = true;
for (auto const &open : open_list) {
if (child == open && child.get_g() > open.get_g()) {
add = false;
break;
}
}
if (add) {
open_list.push_back(child);
}
}
}
return {};
}
int main() {
Graph graph = {
{0, 0, 0, 0, 1, 0, 0},
{0, 3, 0, 0, 1, 0, 2},
{0, 0, 0, 0, 1, 0, 0},
{0, 0, 0, 0, 1, 0, 0},
{0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 1, 0, 0},
{0, 0, 0, 0, 1, 0, 0},
};
int start_row = 0;
int start_column = 0;
std::tie(start_row, start_column) = get_start(graph);
int end_row = 0;
int end_column = 0;
std::tie(end_row, end_column) = get_end(graph);
auto user = Node(start_row, start_column);
auto goal = Node(end_row, end_column);
std::cout << user << std::endl;
std::cout << goal << std::endl;
auto nodes = a_star(graph, user, goal);
std::cout << "Path: [";
for (auto const &node : nodes) {
std::cout << "("
<< std::get<0>(node.get_position())
<< ", "
<< std::get<1>(node.get_position())
<< "), ";
}
std::cout << "]" << std::endl;
return 0;
}
When I run this on my computer, I get this output:
Node((1, 1), 0, 0, nullptr)
Node((1, 6), 0, 0, nullptr)
Path: [(1, 6), ]
Path: should have returned a full list starting from (1, 1) to the goal, (1, 6) But instead of [(1, 1), (1, 2), (2, 3), (3, 3), (4, 4), (3, 5), (2, 6), (1, 6), ]. we got [(1, 6), ]. I think the reason for this is because current_node contains itself as a parent. So trace breaks early to avoid a cyclic loop. And that's why there's only one in the output. It's weird that any Node would have itself as a parent because the constructor throws an exception if that happens.
In fact on my machine, every parent of every single node is the same address, 0x7fffffff9140, which shouldn't be happening.
My only guess to this is that current_node is being initialized in the while loop so maybe even though the Node position / f / g / h are changing, the pointer in memory never does. So all the nodes end up getting the same parent that way.
That may or may not be the issue but either way, I'm not sure how to fix it. Any advice would be helpful. Thank you!

Answer 1

This constructor body is totally messed up, you are reading from member this->parent which is always initialized to nullptr (by the brace-or-equal-initializer).
Node(int row, int column, Node *parent) : row(row), column(column)
{
if (this->parent != nullptr && *this->parent == *this) {
throw "Node cannot be parented to itself";
}
this->parent = parent;
}
Second major problem, this is passing a reference to an object current_node whose lifetime is insufficient:
auto children = get_children(current_node, graph);
By the time you read any of the "children" out of the list, the node pointed to by their parent member has already been destroyed.
The copy in closed_list would survive, but that can't be used to store a parent pointer either, because of the iterator invalidation rules for std::vector. If you change closed_list to std::list<Node> then the element addresses would be stable.
But then when a_star returns all the objects pointed to by instance parent pointers will die anyway. trace(current_node) results in a collection of nodes whose parent members point outside the collection.
Your best bet is probably to preallocate enough Node objects for the whole graph instance, use an index instead of a pointer as parent, and update the parent member instead of setting it during construction. Index values, unlike pointers, remain meaningful no matter how many times a data structure is copied.
(Also, string literals make bad exception objects)