title: A star algorithm in c++
body: <p>I am trying to implement A* search algorithm with priority_queues and I have drawn the output on the console with opengl.</p>
  
  <p>I got this output: <a href="https://i.stack.imgur.com/0rbCZ.jpg" rel="nofollow noreferrer">Final path with A star</a></p>
  
  <p>Where the cells in gray weren't visited, in yellow were visited and the final path in blue.</p>
  
  <p>I think that the cells that were visited should be less and closer to the path, or I am wrong?</p>
  
  <p>I computed the path with the following code:</p>
  
  <pre><code>list&lt;Node*&gt; compute_path_A_star(Node* start, Node* goal)
  {
      priority_queue&lt; nodeDistance, vector&lt; nodeDistance &gt;, CompareDist&gt; pq;
      vector&lt;Node*&gt; came_from;
      vector&lt;float&gt; cost_so_far; 
  
      nodeDistance first;
      first.node = start;
      first.distance = 0.0f;
      pq.push(first);
  
      came_from.resize(20 * 20, nullptr);
  
      cost_so_far.resize(20 * 20, 9999.0f); 
      cost_so_far[start-&gt;x + 20 * start-&gt;y] = 0.0f;
  
      //Compute where we came from for every location that’s visited
      int i = 0;
      while (!pq.empty())
      {
          nodeDistance temp = pq.top();
          pq.pop();
          Node* current_node = temp.node;
          current_node = get_node(current_node-&gt;x, current_node-&gt;y);
  
          if (current_node == goal) break;
  
          for (auto n : current_node-&gt;neighbours)
          {
              float new_cost = cost_so_far[current_node-&gt;x + 20 * current_node-&gt;y] + n-&gt;cost;
  
              if (new_cost &lt; cost_so_far[n-&gt;x + 20 * n-&gt;y])
              {
                  cost_so_far[n-&gt;x + 20 * n-&gt;y] = new_cost;
                  came_from[n-&gt;x + 20 * n-&gt;y] = current_node;
  
                  nodeDistance newNode;
                  newNode.node = n;
                  newNode.distance = new_cost + heuristic(n, goal);
                  pq.push(newNode);
  
              }
          }
      }
  
  
  
      #pragma region Create the path (start -&gt; goal)
  
          Node* current = new Node();
          current = goal;
          list&lt;Node*&gt; path;
          path.push_back(current);
          while (current != start)
          {
              current = came_from[current-&gt;x + 20 * current-&gt;y];
  
              path.push_back(current);
          }
          path.reverse();
  
      #pragma endregion
  
      return path;
  }
  </code></pre>
  
  <p>I used Manhattan distance for the heuristic:</p>
  
  <pre><code>float heuristic(Node* a, Node* b)
  {
      //Manhattan distance on a square grid
      return (abs(a-&gt;x - b-&gt;x) + abs(a-&gt;y - b-&gt;y));
  }
  </code></pre>
  
  <p>nodeDistance is a struct:</p>
  
  <pre><code>struct nodeDistance
  {
      Node* node;
      float distance;
  };
  </code></pre>
  
  <p>And for the comparision in the priority queue I defined this class:</p>
  
  <pre><code>class CompareDist
  {
  public:
      bool operator()(nodeDistance&amp; n1, nodeDistance&amp; n2)
      {
          if (n1.distance &gt; n2.distance)
              return true;
          else
              return false;
      }
  };
  </code></pre>
  
  <p>I defined each node as:</p>
  
  <pre><code>struct Node
  {
      int x, y;
      float cost;
      Node* parent;
      list&lt;Node*&gt; neighbours;
  };
  </code></pre>
  
  <p>And the graph:</p>
  
  <pre><code>list&lt;Node*&gt; Graph;
  </code></pre>
  
  <p>Edit 1: </p>
  
  <p>For the A star implementation, I was based on this page <a href="http://www.redblobgames.com/pathfinding/a-star/introduction.html" rel="nofollow noreferrer">http://www.redblobgames.com/pathfinding/a-star/introduction.html</a>.</p>
  
  <p>There is something that I missed? Thanks! </p>
  
  <p>Edit 2:</p>
  
  <p>I have inserted some obstacles (dark-gray cells) and waypoints (pink cells) on the scene. In red the goal cell</p>
  
  <p><a href="https://i.stack.imgur.com/xvy9R.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/xvy9R.jpg" alt="enter image description here"></a></p>