# C++ - Astar search algorithm using user defined class

The objective of the post is to improve design and improve command on C++. Given a map, start point and end point, the shortest path to end point from the start point has to be found out using Astar (A*) algorithm. The algorithm uses an evaluation function f for each point or node in the map. The f depends on g and h. The g is distance from start point to the point under consideration(current point in the path). The h is a heuristic which provides an (under)estimate of the distance from current point to end point. The frontier is a set that contains all candidate points to form the path. The pseudocode/ algorithm:

1) The values of f, g and h for all points are initially set to infinity (a very high value);
2) The start point is assigned a g of 0 and its h is calculated using Manhattan distance. This is then inserted to the frontier. The end point is assigned an h of 0.
3) Until frontier is empty, do:
a) Extract the point (current point) with lowest f value from the frontier.
b) Check if it is the end point. If yes, report success. Else, continue.
c) Collect all the eligible neighbors of the current point and add them to the frontier. During this addition, their f, g, h and parent are updated.
d) Remove the current point from frontier.
4) If success is not reported in (3), report failure.


Here is the commented code:

#include <iostream>
#include <vector>
#include <stdexcept>
#include <set>
#include <algorithm>

// Class to handle individual nodes/ points/ location in the environment map
class Point
{
// The x coordinate of the point
int x_v = {-1};

// The y coordinate of the point
int y_v = {-1};

// The value at the point; either 1 or 0
int val_v = {0};

// The total estimated cost of a point; A star uses this value
double f_v = {100000};

// The cost to reach a point; A star uses this value
double g_v = {100000};

// The estimate of cost (heuristic) to reach end from current point; A star uses this value
double h_v = {100000};

// The parent of Point set by Astar so that path from start to end can be retrieved
Point* parent_v = nullptr;
public:
Point()
{}

Point(int xx, int yy, int vv) : x_v{xx}, y_v{yy}, val_v{vv}
{}

// Copy constructor
Point(const Point& p1)
{
x_v = p1.x();
y_v = p1.y();
val_v = p1.val();
f_v = p1.f();
g_v = p1.g();
h_v = p1.h();
parent_v = p1.parent();
}

~Point(){}

int val() const
{
return val_v;
}

int x() const
{
return x_v;
}

int y() const
{
return y_v;
}

double f() const
{
return f_v;
}

double g() const
{
return g_v;
}

double h() const
{
return h_v;
}

Point* parent() const
{
return parent_v;
}

void set_g(double g)
{
g_v = g;
f_v = g_v + h_v;
}

void set_h(double h)
{
h_v = h;
f_v = g_v + h_v;
}

void set_parent(Point* p)
{
parent_v = p;
}

// Assignment operator
Point& operator=(const Point& p1)
{
x_v = p1.x();
y_v = p1.y();
val_v = p1.val();
f_v = p1.f();
g_v = p1.g();
h_v = p1.h();
parent_v = p1.parent();
return *this;
}

//This operator has been defined so that std::set can use it as comparison object
friend bool operator<(const Point& p1, const Point& p2)
{
if(p1.x() < p2.x())
{
return true;
}
if(p1.x() == p2.x() && p1.y() < p2.y())
{
return true;
}
return false;
}

friend bool operator==(const Point& p1, const Point& p2)
{
return (p1.x() == p2.x()) && (p1.y() == p2.y());
}

friend bool operator!=(const Point& p1, const Point& p2)
{
return !(p1 == p2);
}
};

// Class to perform A star
class Astar
{
// The map of the environment
std::vector<std::vector<Point>> map_v;

// The size of the map
int map_x = {0};
int map_y = {0};

// The start and end points
Point* start_v;
Point* end_v;

// The variable to store path from start to end
std::vector<Point*> path_v;
public:
Astar(std::vector<std::vector<int>>&, std::pair<int, int>&, std::pair<int, int>&);
bool is_valid(int, int);
double manhattan(Point*);
bool search();
std::vector<Point*> path();
};

// Constructor that takes in map, start and end from the user/ main and converts it into variables of the class
Astar::Astar(std::vector<std::vector<int>>& map, std::pair<int, int>& start, std::pair<int, int>& end)
{
// Check and note down sizes
map_y = map.size();
if(map_y)
{
map_x = map.size();
}
if(map_x == 0 || map_y == 0)
{
throw std::invalid_argument{"The map is invalid!\n"};
}

// Create a map of Points
for(int i = 0; i < map_y; i++)
{
map_v.push_back(std::vector<Point>(map_x));
for(int j = 0; j < map_x; j++)
{
map_v[i][j] = Point(j, i, map[i][j]);
}
}

// Assign start and end
start_v = &map_v[start.first][start.second];
end_v = &map_v[end.first][end.second];
if(!is_valid(start_v -> x(), start_v -> y()))
{
throw std::invalid_argument{"Start point is invalid!\n"};
}
if(!is_valid(end_v -> x(), end_v -> y()))
{
throw std::invalid_argument{"End point is invalid!\n"};
}
}

// Check if a Point lies within boundaries of the map and if it is free space
bool Astar::is_valid(int x, int y)
{
if(x >= 0 && x < map_x && y >= 0 && y < map_y && map_v[y][x].val() == 0)
{
return true;
}

return false;
}

// Calculate Manhattan distance as a hueristic
double Astar::manhattan(Point* p)
{
return std::abs(p -> x() - end_v -> x()) + std::abs(p -> y() - end_v -> y());
}

// Perform the actual search
bool Astar::search()
{
// Create a frontier and insert the start node
std::set<Point*> frontier;
end_v -> set_h(0);
start_v -> set_g(0);
start_v -> set_h(this -> manhattan(start_v));
frontier.insert(start_v);

// As long as there are points in the frontier or until the end point is reached, the search continues
while(!frontier.empty())
{
// Find the Point with minimum value of f_v
auto curr_point = *(std::min_element(frontier.begin(), frontier.end(), [](const Point* p1, const Point* p2){return p1 -> f() < p2 -> f();}));

// If it is the end point, return success
if(*curr_point == *end_v)
{
std::cout << "Success!\n";
return true;
}

// Otherwise, find the eligible neighbors and insert them into frontier
int x = curr_point -> x();
int y = curr_point -> y();
std::vector<Point*> neighbors;
if(this -> is_valid(x, y - 1))
{
neighbors.push_back(&map_v[y - 1][x]);
}
if(this -> is_valid(x, y + 1))
{
neighbors.push_back(&map_v[y + 1][x]);
}
if(this -> is_valid(x + 1, y))
{
neighbors.push_back(&map_v[y][x + 1]);
}
if(this -> is_valid(x - 1, y))
{
neighbors.push_back(&map_v[y][x - 1]);
}

// Add neighbors to frontier if their g value is higher than necessary
// Update g, h (and f). Also set their parent.
for(auto& neighbor : neighbors)
{
if(neighbor -> g() > curr_point -> g() + 1)
{
neighbor -> set_g(curr_point -> g() + 1);
neighbor -> set_h(this -> manhattan(neighbor));
neighbor -> set_parent(curr_point);
frontier.insert(neighbor);
}
}
// Remove the current Point
frontier.erase(curr_point);
}
// If end point is not reached, report failure
std::cout << "Failure!\n";
return false;
}

// Retrieve the path and return it
std::vector<Point*> Astar::path()
{
auto p1 = end_v;
while(p1 != nullptr)
{
path_v.insert(path_v.begin(), p1);
p1 = p1 -> parent();
}
return path_v;
}

int main()
{
// Map of the environment to navigate
std::vector<std::vector<int>> mv = {{1, 0, 0, 1, 0, 0, 0, 0},
{0, 1, 0, 1, 1, 0, 0, 1},
{1, 0, 0, 0, 0, 0, 1, 0},
{0, 0, 1, 0, 0, 0, 0, 0},
{1, 0, 1, 0, 1, 1, 1, 1},
{0, 0, 0, 0, 0, 1, 0, 1},
{0, 0, 0, 0, 0, 1, 1, 0},
{0, 1, 0, 1, 0, 0, 0, 1}};
// The start and end points
std::pair<int, int> start = {5, 2};
std::pair<int, int> end = {2, 5};

// Create search object and perform search
Astar astar1{mv, start, end};
auto success = astar1.search();

// If search is successful, print the path
if(success)
{
auto path = astar1.path();
std::cout << "The result path: \n";
for(auto p : path)
{
std::cout << "{ " << p -> y() << ", " << p -> x() << "}" << "\t";
}
std::cout << "\n";
}

return 0;
}


1) In the above code, the user/ main provides input map via a std::vector<std::vector<int>> and it is converted to std::vector<std::vector<Point>>. The Point is a user defined class capable of handling a lot of operations. In the above code, the class Astar is responsible for conversion of the map into required type. Is this bad design? How can I can do it more elegantly without loosing the advantages of Point class? Suggestions for better data structures are welcome.

2) Comments on operator overloading, copy constructor etc. used in Point.

3) Are there any parts that are very inefficient? Any other suggestions, advice or improvements?

A std::vector<std::vector<int>> is an inefficient way to store a 2-dimensional array. Unfortunately, there is nothing in the standard C++ library that gives you an easy and safe 2-dimensional array.

However, in your case, in main() you could easily convert mv into a straightforward 2D array, like so:

int mv = {{...same as you already have...}};


But then you'd have to pass both a pointer to that array and its dimensions to any class/function that uses it. It is not too hard to write a 2D array class yourself though, or search for some external library that implements one for you. Or stick with this for now.

As for the design with the classes Point and Astar, comments on overloading and so on, and other suggestions, see below.

## Unnecessary hiding of variables

If you have a class that has variables that you should be able to read and set at will, and there are no side-effects or restrictions, then why not make them public? It saves you writing lots of getters and setters. So:

class Point {
public:
int x = -1;
int y = -1;
...
};


Or just write struct Point, which will make everything public by default.

## Don't use initializer lists unnecessarily

It looks like you are mixing two styles of default initialization of member variables. You should either write:

int x = -1;


Or:

int x{-1};


What you write is actually an initializer list of one element.

## Don't write an operator/constructor/destructor if it's equivalent to the default one

Your copy constructor does exactly what the default copy constructor would do, so there is no point in writing it out. It is only a potential source of errors and inefficiencies.

The same also applies to the destructor and the assignment operator.

## Consider making Point part of Astar

Your Point class is specifically made for the A* algorithm. It then makes sense have it part of the namespace of the Astar class. Just move it inside the latter:

class Astar {
public:
class Point {
...
};

...
};


However, the class Astar is problematic in itself:

## Don't conflate the algorithm and its input

Your class Astar is both the map and the methods to perform the A* algorithm. In a real application, the map is some datastructure used by many algorithms, and you want the A* algorithm to work on that datastructure. So it is much more natural to have a class Map, and a function Astar(...) that takes a Map, and the start and endpoints as arguments, and returns the resulting path.

## Have a single function that calculates and returns a path

Your class requires you to do things in three stages:

• construct Astar and have it copy the map data
• call search() to have it calculate the path
• call path() to retrieve the path

This is quite cumbersome. You typically want a single function that performs all these steps in one go, something that looks like:

std::vector<Point> Astar(const Map &map, const Point &start, const Point &end);


And those Points there should just be something that looks like std::pair<int, int>, and not contain any of the variables used by the A* algorithm internally.

The above function can signal that it didn't find a path by returning an empty vector.

## Avoid returning raw pointers

Your function path() returns a std::vector<Point *>. It looks like a return by value, but this vector contains raw pointers to the data held by a variable of class Astar. However, you now introduce a potential issue: if the class Astar variable goes out of scope, the path vector now points to invalid memory. There are several solutions to this:

• Return a pointer/reference to the vector inside class Astar (this still can be problematic, but now it is much more clear that it is just a pointer)
• Return a deep copy (std::vector<Point>)
• Use std::shared_ptr()

## Avoid writing this->foo

Just write foo directly. For example, in Astar::search(), just call is_valid(x, y) instead of this->is_valid(x, y).

## Minor style issues

You are putting spaces around the -> operator, but not around .. It is very uncommon to do that, just write foo->bar without spaces.

Instead of map_x and map_y, write width and height. When iterating over the map coordinates, use x and y as iterators instead of i and j.