My A* Search Code Does Not Give the Correct Answer

Hello there, I am trying to create an mfile for A* Search Algorithm. I tried the script on two different graphs and in both of them algorithm does not give the correct answer. Here is an example graph in which I tried my code:

**Optimist CTG Field refers to the heuristic values of the nodes.

The solution to the graph is : 1 - 4 - 3 - 5 - 6.

I tried to implement this pseudocode:

I created the OPEN and CLOSED lists as cell arrays. Each cell consists of the nodes. I constructed the notes as structures with parent, total cost, heuristic, successors/adjacent, and node number information.

My cell array looks like this:

And each struct in the cells looks like this (screenshot below just shows the node1:

Finally, here is my code:

clc
clear

%% Constructing the Nodes with their information:
%  Assigning the adjacent node information to nodes:
node(1).successors = [3 4 5];
node(2).successors = [3 6];
node(3).successors = [1 2 6];
node(4).successors = [1 5 6];
node(5).successors = [1 6];
node(6).successors = [2 3 4 5];

%  Assigning heuristic cost values and cost numbers to nodes:
node(1).heuristic = 20;
node(1).order = 1;
for i = 2 : 6
    node(i).heuristic = 10;
    node(i).order = i;
end

%  Reading the edge information from edges file:
edges = readmatrix('edgesdeneme.csv')

%  Assigning past cost and total cost values to nodes:
node(1).pastCost = 0;
node(1).totalCost = node(1).pastCost + node(1). heuristic;
for i = 2 : 6
    node(i).pastCost = Inf;
    node(i).totalCost = node(i).pastCost + node(i). heuristic;
end

%  Setting the start and node goals as well as the parent of node 1:
node(1).parent = [];
nodeStart = 1;
nodeGoal = 6;

%  Adding the first node to OPEN List while creating an empty CLOSED List
OPEN = {node(1)};
CLOSED = {};

%% Starting the A* Search Algorithm:
%  If tthe OPEN list is not empty, iterations are going to start:
while(~isempty(OPEN))
  current = OPEN{1}
  OPEN(1) = [];
  CLOSED{length(CLOSED) + 1} = current;
  
  %  If the curret node is the goal node, algorithm is finished with
  %  success, return the path from the CLOSED List:
  if (current.order == nodeGoal)
      disp('SUCCESS!')
      
      for i = 1 : length(CLOSED)
          
          fprintf(' %d ', CLOSED{i}.order)
          
      end
      return
      
  end
  
  % Creating an array which consists of the node numbers of the closed nodes:
        for i = 1 : length(CLOSED)
        closedNodes(i) = CLOSED{i}.order;
        end
  
  % Starting searching through the adjacents/successors of the current node:      
  for i = 1 : length(current.successors)
      %  If the current node is not in the CLOSED List / member of the
      %  closedNodes array, calculations are going to be taking place:
      if (~ismember(current.successors(i), closedNodes))
          
          tentativePastCost = current.pastCost + edgeCost(current.order, current.successors(i), edges)
          
          if (tentativePastCost < node(current.successors(i)).pastCost)
              node(current.successors(i)).pastCost = tentativePastCost;
              node(current.successors(i)).parent = current.order;
              
              
              node(current.successors(i)).totalCost = node(current.successors(i)).pastCost + node(current.successors(i)).heuristic;
              disp(node(current.successors(i)).totalCost)
              %  Adding the current node to the OPEN List and then sorting
              %  the OPEN List with respect to total cost values:
              OPEN{length(OPEN) + 1} = node(current.successors(i));
              [~,I] = sort(cellfun(@(s) getfield(s,"totalCost"), OPEN));
              OPEN = OPEN(I);
              
          end
          
      end
      
  end
  
end

Output of my script is this:

SUCCESS!
 1  4  3  5  5  6 >>

It shows the 5th node twice. I controlled my code and I see two different node 6 in my CLOSED List, with 2 different total cost values. What can my mistake be?

[Bruno Luong]

Not sure the algorithm you posted is correct, in the pseudo code from Wikipedia I read

"if neighbor not in openSet

openSet.add(neighbor)"

You seems to add neighbor to OPEN regardless it's already there or not.

[Kaan Uçar]

Yes, you are right, it was not mentioned in the pseudocode that I was using. Thank you so much!

[Bhanu Teja]

HEY HOW TO CHECK WHETHER NEIGHBOUR IS IN OPEN ON NOT?

STRUCK THERE

[Kaan Uçar]

You can just create an array of the node numbers that are in the closed list and then use ismember function. But first we need to know what data type are you using for the Open - Closed lists.