You can use a dictionary I call queuedWaypoints where the key is the Vector2Int coordinate that has been queued, and the value is the waypoint from which the coordinate was visited from.
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class Pathfinder : MonoBehaviour
{
[Header("Key Waypoints")]
[SerializeField] Waypoint startWaypoint;
[SerializeField] Waypoint endWaypoint;
[Header("Color settings")]
[SerializeField] Color routeColor = Color.cyan;
[SerializeField] Color startColor = Color.blue;
[SerializeField] Color endColor = Color.red;
[SerializeField] Color foundColor = Color.yellow;
Dictionary<Vector2Int, Waypoint> grid = new Dictionary<Vector2Int, Waypoint>();
Queue<Waypoint> waypointsToVisit = new Queue<Waypoint>();
//dictionary of waypoints that have been queued, and which waypoint it was queued (visited) from
Dictionary<Vector2Int, Waypoint> queuedWaypoints = new Dictionary<Vector2Int, Waypoint>();
Vector2Int[] directions = {
Vector2Int.up,
Vector2Int.right,
Vector2Int.down,
Vector2Int.left,
};
bool endFound = false;
void Start() {
startWaypoint.ChangeWaypointColor(startColor);
endWaypoint.ChangeWaypointColor(endColor);
LoadWaypoints();
StartCoroutine( Pathfind() );
print("line after pathfind executed");
}
void LoadWaypoints()
{
Waypoint[] waypoints = FindObjectsOfType<Waypoint>();
foreach (Waypoint waypoint in waypoints)
{
grid[waypoint.GetSnappedGridPos()] = waypoint;
}
}
IEnumerator Pathfind() {
AddToQueuedAndQueued(startWaypoint.GetSnappedGridPos(), null);
while(waypointsToVisit.Count > 0 && !endFound) {
Waypoint currentWaypoint = waypointsToVisit.Dequeue();
CheckWaypointIsEnd(currentWaypoint);
ExploreNeighboursOf(currentWaypoint);
yield return new WaitForSeconds(0.5f);
}
}
private void ExploreNeighboursOf(Waypoint currentWaypoint)
{
if (endFound) return;
print($"exploring neighbours of {currentWaypoint}");
foreach (Vector2Int direction in directions) {
Vector2Int currentWaypointPosition = currentWaypoint.GetSnappedGridPos();
Vector2Int positionToEnqueue = currentWaypointPosition + direction;
if (!CheckIfValidPoint(positionToEnqueue)) continue;
if (CheckIfAlreadyQueued(positionToEnqueue)) continue;
AddToQueuedAndQueued(positionToEnqueue, currentWaypoint);
}
}
private void CheckWaypointIsEnd(Waypoint currentWaypoint)
{
if (currentWaypoint.Equals(endWaypoint)) {
print("End waypoint found");
endFound = true;
currentWaypoint.ChangeWaypointColor(foundColor);
PrintPathTo(currentWaypoint);
} else {
currentWaypoint.ChangeWaypointColor(routeColor);
}
}
private void PrintPathTo(Waypoint endWaypoint)
{
Stack<Vector2Int> coordinateStack = new Stack<Vector2Int>();
Vector2Int endWaypointPosition = endWaypoint.GetSnappedGridPos();
coordinateStack.Push(endWaypointPosition);
Waypoint nextWaypoint = queuedWaypoints[endWaypointPosition];
while (nextWaypoint != null) {
Vector2Int nextCoord = nextWaypoint.GetSnappedGridPos();
coordinateStack.Push(nextCoord);
nextWaypoint = queuedWaypoints[nextCoord];
}
print("PATH TO FINISH:");
while (coordinateStack.Count > 0) {
print(coordinateStack.Pop());
}
}
private void AddToQueuedAndQueued(Vector2Int positionToEnqueue, Waypoint waypointVisitedfrom)
{
print($"Enqueueing {positionToEnqueue}");
Waypoint waypointToEnqueue = grid[positionToEnqueue];
waypointsToVisit.Enqueue(waypointToEnqueue);
queuedWaypoints[positionToEnqueue] = waypointVisitedfrom;
}
private bool CheckIfValidPoint(Vector2Int positionToCheck)
{
return grid.ContainsKey(positionToCheck);
}
private bool CheckIfAlreadyQueued(Vector2Int positionToCheck)
{
return queuedWaypoints.ContainsKey(positionToCheck);
}
}