Need some help with A* Pathfinding [closed]

Question

I am trying to read in from 2 text files (one contains the start and end coordinates, the other is a map of costs for use with the Manhatten Distance) and I'm really struggling with the implementation of the code from various algorithms I've found.

I've been able to read in the text files and put them into 2 separate arrays fine, the real issue I'm having is with adding to the open list and compares their costs so that the smallest goes onto the priority queue and then compare its neighbors, I just cannot work this out after hours upon hours of reading and watching YouTube videos on the subject, any help?

Here is my code so far:

#include "stdafx.h"
#include <iostream>
#include <string>
#include <fstream>
#include <deque>
#include <math.h>
#include <vector>
#include <deque>
using namespace std;

struct pNode
{
    int xPos;
    int yPos;
    pNode* Parent;
    pNode* child;



    float g; //Cost of this Node plus predecessors
    float h;//Heuristic estimate to Goal state
    float f; // f= g+h



    pNode() :
        Parent(0),
        child(0),
        g(0.0f),
        h(0.0f),
        f(0.0f)
    {
        bool wall = false;
        bool water = false;
        bool grass = false;
        bool startNode = false;
        bool endNode = false;
    }
};

class compare
{
public:

    bool compareNodes()
    {
        pNode *x;
        pNode *y;
        return x->f > y->f;
    }
};

class dMap
{
public:
    static const int ARRAY_SIZE = 100;

    void LoadFile();
    void Display() const;
    void priorityQueue();
    void aStar();

private:
    pNode   mArray[ARRAY_SIZE];
    pNode   mArrayCoords[ARRAY_SIZE];
};


void dMap::LoadFile()
{
    ifstream infile("dMap.txt");
    if (!infile)
    {
        cout << "Cannot open File";
        exit(1);
    }
    for (int i = 0; i < ARRAY_SIZE; i++)
    {
        infile >> mArray[i].xPos >> mArray[i].yPos;
    }
}

void dMap::priorityQueue()
{
    int disX = mArray[ARRAY_SIZE].xPos;
    int disY = mArray[ARRAY_SIZE].yPos;
    int heuristic(int xPos, int yPos);

    struct pQueue
    {

        int fx;
        int fy;
        int x;
        int y;

        int cost;
        int h;
        int totalCost;


        pQueue(int _x, int _y, int _cost)
        {
            x = _x;
            y = _y;
            cost = _cost;
            h = heuristic(x, y);

            totalCost = cost + h;
        }

        int manhattenDistance(int xPos, int yPos)
        {
            return abs(xPos - fx) + abs(yPos - fy);
        }//Manhatten Distance

        bool operator<(const pQueue &b)const
        {
            return totalCost>b.totalCost;
        }

    };
}




void dMap::aStar()
{
    LoadFile();

    pNode mArrayCoords[ARRAY_SIZE];
    ifstream infile("dCoords.txt");
    if (!infile)
    {
        cout << "Cannot open File";
        exit(1);
    }
    for (int i = 0; i <9; i++)
    {
        infile >> mArrayCoords[i].xPos >> mArrayCoords[i].yPos;
    }

    deque <pNode*> openList;
    pNode* startNode = new (pNode);
    deque <pNode*> closedList;
    pNode* endNode = new (pNode);
    pNode* currentNode = new (pNode);
    deque <pNode*> finalList;

    //startNode ->




}

Answer 1

Currently, I am working on the same thing, except that the nodes are hexagons and not squares(I assume you are using squares).

Red Blob Games is a great resource.

2.4 A* Search

A* is almost exactly like Dijkstra’s Algorithm, except we add in a heuristic. Note that the code for the algorithm isn’t specific to grids. Knowledge about grids is in the graph class (SquareGrids in this case) and in the heuristic function. Replace those two and you can use the A* algorithm code with any other graph structure.

inline double heuristic(SquareGrid::Location a, SquareGrid::Location b) {
  int x1, y1, x2, y2;
  tie (x1, y1) = a;
  tie (x2, y2) = b;
  return abs(x1 - x2) + abs(y1 - y2);
}

and

template<typename Graph>
void a_star_search
  (const Graph& graph,
   typename Graph::Location start,
   typename Graph::Location goal,
   unordered_map<typename Graph::Location, typename Graph::Location>& came_from,
   unordered_map<typename Graph::Location, double>& cost_so_far)
{
  typedef typename Graph::Location Location;
  PriorityQueue<Location, double> frontier;
  frontier.put(start, 0);

  came_from[start] = start;
  cost_so_far[start] = 0;

  while (!frontier.empty()) {
    auto current = frontier.get();

    if (current == goal) {
      break;
    }

    for (auto next : graph.neighbors(current)) {
      double new_cost = cost_so_far[current] + graph.cost(current, next);
      if (!cost_so_far.count(next) || new_cost < cost_so_far[next]) {
        cost_so_far[next] = new_cost;
        double priority = new_cost + heuristic(next, goal);
        frontier.put(next, priority);
        came_from[next] = current;
      }
    }
  }
}

The type of the priority values including the type used in the priority queue should be big enough to include both the graph costs (cost_t) and the heuristic value. For example, if the graph costs are ints and the heuristic returns a double, then you need the priority queue to accept doubles. In this sample code I use double for all three (cost, heuristic, and priority), but I could’ve used int because my costs and heuristics are integer valued.

Minor note: It would be more correct to write frontier.put(start, heuristic(start, goal)) than frontier.put(start, 0) but it makes no difference here because the start node’s priority doesn’t matter. It is the only node in the priority queue and it is selected and removed before anything else is put in there.

#include "redblobgames/pathfinding/a-star/implementation.cpp"

int main() {
  GridWithWeights grid = make_diagram4();
  SquareGrid::Location start{1, 4};
  SquareGrid::Location goal{8, 5};
  unordered_map<SquareGrid::Location, SquareGrid::Location> came_from;
  unordered_map<SquareGrid::Location, double> cost_so_far;
  a_star_search(grid, start, goal, came_from, cost_so_far);
  draw_grid(grid, 2, nullptr, &came_from);
  std::cout << std::endl;
  draw_grid(grid, 3, &cost_so_far, nullptr);
  std::cout << std::endl;
  vector<SquareGrid::Location> path = reconstruct_path(start, goal, came_from);
  draw_grid(grid, 3, nullptr, nullptr, &path);
}

2.4.1 Straighter paths

If you implement this code in your own project you might find that some of the paths aren’t as “straight” as you’d like. This is normal. When using grids, especially grids where every step has the same movement cost, you end up with ties: many paths have exactly the same cost. A* ends up picking one of the many short paths, and very often it doesn’t look good to you. The quick hack is to break the ties, but it’s not entirely satisfactory. The better approach is to change the map representation, which makes A* a lot faster, and also produces straighter, better looking paths. However, that only works for mostly-static maps where every step has the same movement cost. For the demos on my page, I’m using a quick hack, but it only works with my slow priority queue. If you switch to a faster priority queue you’ll need a different quick hack.

2.4.2 TODO Heuristic function should be template parameter

I should make the heuristic into a template parameter instead of being a global function.

Digging around here should help, if you haven't already.

Answer 2

To keep the answer simple as possible, I will use pseudo code:

start: findPath {

    openList.push(start)
    While (current != end) {
       if openList is empty
           error: no path found
        get current.neighbours
        for each neighbour {
            //prevents the same cell being looked at twice
            if neighbour is not in closedList {
                if neighbour is wall OR outside maze 
                    discard neighbour
                else 
                    Add to open list
            }
        }
        sort openList by heuristic (lowest to front)
        current = openlist.front
        openList.pop_front
    }

    path = closedlist
End: findPath

This algorithm will ensure you always find the shortest path.

Further improvements can be performed such as jump points, but I think you should wrap your head around the simple version first.

To do the sorting, it's pretty simple you store the nodes in a vector, like so:

std::vector<node> OpenList;

Then simply call:

std::sort(OpenList.begin(), OpenList.end(), [](node a, node b) {
    return b.h < a.h;   
});

Once all neighbours are inserted.

This way, all you need is a data representation of your maze (walkable/unwalkable areas and boundaries, and any modifiers to f/g scores). You will use this representation to compute your H score for each neighbour, and if the node is walkable.

Due to this, you do not want to parse from file directly into the open list, as it's size is important.