C++ A Star Algorithm [path takes wrong route] using <deque> & unique_ptr

Question

I am having issues, trying to get my A Star algorithm working, the main problem is with my process of the algorithm, I am using something known as the TL engine which has some of its own datatypes and functions but for the most part, I am 90% certain my problem is with my C++ code and not with TL-engine function, specifically with my understanding / implementation of the algorithm.

Figures and Information

My grid is a 10x10 grid, called SNodeCubeArray[10][10].

My program reads a txtfile in and sets a base value which determines which bools it will set.

When I generate new rules on the Algorithm I go, I visit the neighbours NESW, and to travel to each node I have a cost assigned to different node types.

Here is an example:

• Normal Nodes always have a cost = 1
• Woods Cost = 2
• Water Cost = 3
• Obstacle / wall = 0 (but is impassable)

Figure 3 explains what happening in more detail.

When I run my AStar algorithm, it performs a very strange path of opennodes, often jumping around and also causing dereferencing iterator problems.

Some functions / variables explanation:

gDEST_X & gDEST_Y = dsetination node stored
gSTART_X & gSTART_Y = start node stored
ANodeCubeModels = Model Array

This is the struct I have declared it is the datatype I'll be using with Unique_ptr's deque.

// Code by ryan newell uclan student G20618255  -> for self reference
struct node
{

public:
  // The Structs actual location in the array eg [3,3]
  int mLoc_X; // Current X- Position in the Node stored
  int mLoc_y; // Current Y- Position in the node stored

  int mHeuristic_Manhattan; // Is the stored nodes manhattan distance

  // These floats are so I can store the coordinates of the linked cubes it should be in.

  //Temporary storage for the string reading. 
  char mNodeValue;

  // Declares what kind node it is. 
  bool mIsObstacle = false;
  bool mIsWater = false;
  bool mIsWoods = false;
  bool mIsStartNode = false;
  bool mIsDestNode = false;

  //Cost storage heurestic 
  int mNodeCost; // The cost to travel between the nodes
  int mAccumulatedCost; // The accumulated cost of the node + its parent

  int mvalue_f; // Is its total score which will equals NodeCost+Heuristic

  //pARENT
  node* m_parent;
};

This is my functions section of the code being used, that we need to know about, any functions I haven't likely are TL-engine functions, and tested to work.

// sorts the node array by order of total_score
bool CompareNodeScores(unique_ptr<node>& lhs, unique_ptr<node>& rhs)
{
  return lhs->mvalue_f < rhs->mvalue_f;
}

bool check_greater_fcost(
    deque <unique_ptr< node >> &closeList, 
    unique_ptr <node> &NewState_n, 
    deque <unique_ptr<node>> &openList)
{
  if (openList.empty() == false)
  {
    for (auto it = openList.begin(); it != openList.end(); it++)
    {
      if ((*it)->mLoc_X == NewState_n.get()->mLoc_X && 
          (*it)->mLoc_y == NewState_n.get()->mLoc_y && 
          NewState_n->mvalue_f >= (*it)->mvalue_f)
        return true;

      return false;
    }
  }
  if (closeList.empty() == false)
  {
    for (auto it = closeList.begin(); it != closeList.end(); it++)
    {
      if ((*it)->mLoc_X == NewState_n.get()->mLoc_X && 
          (*it)->mLoc_y == NewState_n.get()->mLoc_y && 
          NewState_n->mvalue_f >= (*it)->mvalue_f)
        return true;

      return false;
    }
  }
}

bool astar_checklists(
  deque <unique_ptr< node >> &closeList, 
  unique_ptr <node> &RuleCurrent, 
  deque <unique_ptr<node>> &openList, 
  unique_ptr <node> &CurrentNode)
{
  // set p to the beginning of the loop
  for (auto it = openList.begin(); it != openList.end(); it++)
  {
    if ((*it)->mLoc_X == RuleCurrent.get()->mLoc_X && 
        (*it)->mLoc_y == RuleCurrent.get()->mLoc_y)
    {
      (*it)->mAccumulatedCost = RuleCurrent->mAccumulatedCost;
      (*it)->mHeuristic_Manhattan = RuleCurrent->mHeuristic_Manhattan;
      (*it)->mNodeCost = RuleCurrent->mAccumulatedCost;
      (*it)->mvalue_f = RuleCurrent->mHeuristic_Manhattan + RuleCurrent->mNodeCost;
      (*it)->m_parent = CurrentNode->m_parent;
      return true;
    }
  }
  // set p to the beginning of the loop
  for (auto it = closeList.begin(); it != closeList.end(); it++)
  {
    if ((*it)->mLoc_X == RuleCurrent.get()->mLoc_X && 
        (*it)->mLoc_y == RuleCurrent.get()->mLoc_y)
    {
      (*it)->mAccumulatedCost = RuleCurrent->mAccumulatedCost;
      (*it)->mHeuristic_Manhattan = RuleCurrent->mHeuristic_Manhattan;
      (*it)->mNodeCost = RuleCurrent->mAccumulatedCost;
      (*it)->mvalue_f = RuleCurrent->mHeuristic_Manhattan + RuleCurrent->mNodeCost;
      (*it)->m_parent = CurrentNode->m_parent;

      openList.push_back(move(*it));
      closeList.erase(it);
      sort(openList.begin(), openList.end(), CompareNodeScores);
      return true;
    }
    else
    {
      return true;
    }
  }
  return false;
}

This section is where the main body of the algorithm itself lies, I am not really sure what I am doing wrong, but when I run the program, it does not take the correct route it should take.

void AStarFindPathNew(
  I3DEngine* myEngine, 
  IModel* ANodeCubeModels[10][10], 
  node SNodeArray[10][10], 
  int storemLoc_X, 
  int storemLoc_y, 
  int storedestmLoc_x, 
  int storedestmLoc_y)
{
  int xstore = 0;
  int ystore = 0;
  int xplus = 0;
  int yplus = 0;
  bool goal_state_found = false;
  //bool current_is_on_openlist = false;
  //bool current_is_on_closelist = false; previous unused values
  //bool current_is_on_bothlists = false;
  ofstream outfile;
  unique_ptr <node> NewState_n(new node);
  unique_ptr <node> CurrentNode(new node);
  unique_ptr <node> initialState(new node);
  deque <unique_ptr < node > > openList;
  deque <unique_ptr < node > > closeList;

  // Declaring Intial state
  initialState->mLoc_X = gSTART_X;
  initialState->mLoc_y = gSTART_Y;
  initialState->m_parent = 0;
  initialState->mAccumulatedCost = 0;
  initialState->mHeuristic_Manhattan = CalculateManhattan( ANodeCubeModels, 
                                                           SNodeArray, 
                                                           initialState->mLoc_X, 
                                                           initialState->mLoc_y, 
                                                           storedestmLoc_x, 
                                                           storedestmLoc_y);
  initialState->mvalue_f = initialState->mHeuristic_Manhattan + initialState->mNodeCost;
  DisplayOpenlist(openList, outfile);
  openList.push_front(move(initialState));
  xstore = gSTART_X;
  ystore = gSTART_Y;

  while (!goal_state_found || !openList.empty())
  {
    // Values Value_G = Manhattan Distance = mHeuristic_Manhattan
    // Value: Value_MC = Movement Cost Heuristic  = mNodeCost
    // Value: Value_F = H + MC = m_valuef
    CurrentNode = (move(openList.front()));
    cout << "popped open to New current Node = " << 
      CurrentNode->mLoc_X << ":" << CurrentNode->mLoc_y << endl;
    openList.pop_front();
    //cout << CurrentNode->mLoc_X << "," << CurrentNode->mLoc_y << "  Target: " 
    // << gDDEST_X << "," << gDDEST_Y << endl;
    if (CurrentNode->mLoc_X == storedestmLoc_x && CurrentNode->mLoc_y == storedestmLoc_y)
    {
      cout << "Found Path" << endl;
      _getch();
      fRetracesteps(myEngine, ANodeCubeModels, SNodeArray, CurrentNode);
      return;
    }
    DisplayOpenlist(openList, outfile);

    for (int i = 0; i < 4; i++)
    {
      if (i == 0)
      {
        xplus = 0;
        yplus = +1;
      }
      if (i == 1)
      {
        xplus = +1;
        yplus = 0;
      }
      if (i == 2)
      {
        xplus = 0;
        yplus = -1;
      }
      if (i == 3)
      {
        xplus = -1;
        yplus = 0;
      }
      cout << "Generating Rule X:" << xstore + xplus << "Y:" << ystore + yplus << endl;
      //d) i Generate N 
      NewState_n.reset(new node);
      NewState_n->mLoc_X = CurrentNode->mLoc_X + xplus;
      NewState_n->mLoc_y = CurrentNode->mLoc_y + yplus;
      NewState_n->mAccumulatedCost = CurrentNode->mAccumulatedCost + 
                   SNodeArray[NewState_n->mLoc_X][NewState_n->mLoc_y].mHeuristic_Manhattan;
      NewState_n->mHeuristic_Manhattan = CalculateManhattan(ANodeCubeModels, SNodeArray, 
                 NewState_n->mLoc_X, NewState_n->mLoc_y, storedestmLoc_x, storedestmLoc_y);
      NewState_n->mvalue_f = NewState_n->mHeuristic_Manhattan + NewState_n->mAccumulatedCost;
      NewState_n->m_parent = CurrentNode.get();
      cout << NewState_n->m_parent;

      cout << "Checking curent rule for within boundaries" << endl;
      if (!checkcurrentruleinbounds(NewState_n))
      {
        continue;
      }

      NewState_n->mIsObstacle = 
        SNodeArray[CurrentNode->mLoc_X + xplus][CurrentNode->mLoc_y + yplus].mIsObstacle;

      cout << "Checking whether the rule is on the lists or has a greater cost" << endl;
      if (astar_checklists(closeList, NewState_n, openList, CurrentNode) && 
          !check_greater_fcost(closeList, NewState_n, openList))
      {
        continue;
      }

      if (NewState_n->mIsObstacle != true)
      {
        //cout << "Setting newstate_n" << NewState_n.get()->mLoc_X << 
          //NewState_n->mLoc_y << NewState_n->m_parent;
        ANodeCubeModels[NewState_n->mLoc_X][NewState_n->mLoc_y]->SetSkin("opennode.png");
        openList.push_back(move(NewState_n));
        sort(openList.begin(), openList.end(), CompareNodeScores);
        cout << CurrentNode->mLoc_X << ":" << CurrentNode->mLoc_y;
      }

      DisplayOpenlistWithScore(openList, outfile);
    }
    cout << "Finished first set of rules" << endl;
    cout << "Close List:" << CurrentNode->mLoc_X <<":"<< CurrentNode->mLoc_y << endl;
    ANodeCubeModels[CurrentNode->mLoc_X][CurrentNode->mLoc_y]->SetSkin("closednode.png");
    closeList.push_back(move(CurrentNode));

    _getch();
    myEngine->DrawScene();
    DelayProc(0.75);
  }
}

Answer 1

Found the issue with my program, the algorithm was mostly right, I just had to modify my if statements when taking things on and off the closelist to include the the if statement whether it was smaller than the previous route, since we only want to take things off the close list if a better route has been found.

A secondary issue popped up when I realized I was incorrectly allocating terrain types, taking the wrong route round the top because my although my water nodes appeared as water they were actually woods and vice versa.