I’ve written an implementation of A* that relies on sorting nodes by their F score in a sortedSet.
The sorting, in some cases, seems to insert a Node object at the 'Min' value when its compared 'F' value is actually the second lowest rather than the Min, as described. I'm completely baffled as to why this is happening. I believe it's causing the knock-on effect of causing nodeTree.Remove and nodeTree.RemoveWhere to fail, but that might be the actual cause of the issue, I'm honestly not sure - though I wouldn't know how to fix it if it is.
This is the comparer used. I assume it's relatively obvious that I'm not exactly sure how to implement these, but I think this should work as I intend.
public class FValueFirst : Comparer<PathfindingAgent.Node>
{
public override int Compare(PathfindingAgent.Node x, PathfindingAgent.Node y)
{
int result = x.F.CompareTo(y.F);
if (result == 0)
{
result = y.G.CompareTo(x.G);
}
if(x == y)
{
result = 0;
}
return result;
}
}
This is the Node object, for reference.
public class Node
{
public Cell cell;
public float G;
public float H;
public bool Opened;
public bool Closed;
public Node Previous;
public float F { get => G + H; }
}
This is the function it all occurs in. The result is deterministic, thankfully. Depending on the current destID and the particular layout of the grid's obstacles it will always get out of sort on the same iteration.
public void PathTo(Vector3Int destID)
{
SortedSet<Node> nodeTree = new SortedSet<Node>(new FValueFirst());
Vector3Int radius = PathfindingGrid.Instance.GridRadius;
NodeGrid = new Node[radius.x * 2 + 1, radius.y * 2 + 1, radius.z * 2 + 1];
Node startNode = new Node()
{
cell = PathfindingGrid.Cells[CurrentID.x, CurrentID.y, CurrentID.z],
G = 0,
H = 0
};
Node endNode = new Node()
{
cell = PathfindingGrid.Cells[destID.x, destID.y, destID.z],
G = 0,
H = 0
};
Vector3Int sID = startNode.cell.ID;
Vector3Int eID = endNode.cell.ID;
NodeGrid[sID.x, sID.y, sID.z] = startNode;
NodeGrid[eID.x, eID.y, eID.z] = endNode;
if (endNode.cell.IsOccupied) return;
nodeTree.Add(startNode);
int iterations = 0;
while(true)
{
Node node;
node = nodeTree.Min;
node.Closed = true;
nodeTree.RemoveWhere(n => n == node);
if(node == nodeTree.Min)
{
throw new Exception($"Incorrect node was removed from the tree");
}
if (node == endNode)
{
List<Node> chain = BacktraceChain(node);
Debug.Log($"Path found from {CurrentID} to {destID} with score {endNode.G} traversing {chain.Count} cells in {iterations} iterations");
DrawLine(chain, Color.white);
break;
}
List<Node> neighbours = GetNeighbours(node);
foreach(Node neighbour in neighbours)
{
if (neighbour == startNode || neighbour.Closed) continue;
float newg = Vector3Int.Distance(node.cell.ID, neighbour.cell.ID) + node.G;
if (!neighbour.Opened || newg < neighbour.G)
{
neighbour.G = newg;
neighbour.H = ManhattanHeuristic(neighbour, endNode);
neighbour.Previous = node;
if(!neighbour.Opened)
{
nodeTree.Add(neighbour);
neighbour.Opened = true;
}
else
{
nodeTree.RemoveWhere(n => n == neighbour);
nodeTree.Add(neighbour);
}
}
}
iterations++;
}
}
For posterity, I solved the issue - it was due to my inexperience with the SortedList type.
This code, found near the end of the function was to blame
if (!neighbour.Opened || newg < neighbour.G)
{
neighbour.G = newg;
neighbour.H = ManhattanHeuristic(neighbour, endNode);
neighbour.Previous = node;
if(!neighbour.Opened)
{
nodeTree.Add(neighbour);
neighbour.Opened = true;
}
else
{
nodeTree.RemoveWhere(n => n == neighbour);
nodeTree.Add(neighbour);
}
Specifically, an item in a tree cannot have its compared values modified to the point where it no longer compares correctly in that index. The item must first be removed from the list, modified, and readded.
My guess in hindsight is that, though removed immediately after modification, the tree is unable to be sufficiently traversed to access the target item due to the modification.
Thus my solution was to simply re-arrange the block so that the removal and addition occured on either side of the modification respectively, like so:
if (!neighbour.Opened || newg < neighbour.G)
{
if (neighbour.Opened)
{
if (!nodeTree.Remove(neighbour)) throw new Exception($"{neighbour} was not removed from tree");
}
else
{
neighbour.Opened = true;
}
neighbour.G = newg;
neighbour.H = ManhattanHeuristic(neighbour, endNode);
neighbour.Previous = node;
nodeTree.Add(neighbour);
}