My pathfinding code crashes the editor

Hello,

I am making a taxi for my game. What I made so far is a waypoint system and pathfinding for that system.

Today I found out a bug that crashes my entire editor while picking a goal for my taxi. But since my waypoint picking system not working properly too, I am not entirely sure when it actually happens. I can’t fix none of these because of this crash. What I suspect is an endless loop in pathfinding code because when I inactive that part, it doesn’t crash.

Edit: It crashes at random times whenever I call a_star_search method.

Here’s my pathfinding code

PriorityQueue<CarWaypoint> TheQueue; //For priorities
        Dictionary<CarWaypoint, CarWaypoint> CameFrom; //For where we came from to the waypoint
        Dictionary<CarWaypoint, float> CostSoFar; //Cost so far to this waypoint
    
        private void Start() //Default values
        {
            TheQueue = new PriorityQueue<CarWaypoint>();
            CameFrom = new Dictionary<CarWaypoint, CarWaypoint>();
            CostSoFar = new Dictionary<CarWaypoint, float>();
        }
    
        private float heuristic(CarWaypoint a, CarWaypoint b, float CostUntil)
        { //Heuristic function to prioritize waypoints
            return (a.transform.position - b.transform.position).magnitude + (CostUntil + 1) * 2;
        }
    
        public Dictionary<CarWaypoint, CarWaypoint> a_star_search(CarWaypoint start, CarWaypoint goal)
        {
            TheQueue.Clear(); //Clear the entire queue and clear the variables
            CameFrom = new Dictionary<CarWaypoint, CarWaypoint>();
            CostSoFar = new Dictionary<CarWaypoint, float>();
    
            TheQueue.Enqueue(start, 1); //Enqueue starting waypoint
            CameFrom.Add(start, null);
            CostSoFar.Add(start, 0);
            CarWaypoint Frontier;
    
            bool Found = false;
    
            do
            {
                //Get the first value in our prioritized queue
                Frontier = TheQueue.Dequeue();
    
                //My waypoint structure includes three different direction types
                //1. NextWaypoint, which is directly next one.
                //2. Branches, where there is no NextWaypoint and vehicle is entering or exiting a junction
                //3. LaneChanges where it's just a lane change on straight roads
    
                //For NextWaypoint
                if (Frontier.NextWaypoint != null)
                {
                    CheckFor(Frontier.NextWaypoint, Frontier, goal);
                    if (Frontier.NextWaypoint == goal) { Found = true; break; }
                }
                
                //For Branches
                if (Frontier.Branches != null)
                {
                    foreach (CarWaypoint obj in Frontier.Branches)
                    {
                        CheckFor(obj, Frontier, goal);
                        if (obj == goal) { Found = true; break; }
                    }
                }
    
                //For LaneChanges
                if (Frontier.LaneChanges != null)
                {
                    foreach (CarWaypoint obj in Frontier.LaneChanges)
                    {
                        CheckFor(obj, Frontier, goal);
                        if (obj == goal) { Found = true; break; }
                    }
                }
            } while (Frontier != null && Frontier != goal && !Found);
    
            //Loop broken. Either it's been found or not found and finished the entire waypoint system..
            if (Frontier == goal || Found) //Did we found it?
            {
                //Go back from the goal and find the start
                Dictionary<CarWaypoint, CarWaypoint> Path = new Dictionary<CarWaypoint, CarWaypoint>();
                CarWaypoint Last = goal;
                while (Last != start)
                {
                    Path.Add(CameFrom[Last], Last);
                    Last = CameFrom[Last];
                }
                Debug.Log("Path found");
                return Path;
            }
            else
            {
                Debug.Log("Path not found");
                return null;
            }
        }
    
        //This function is to check a waypoint, how close it is to the goal and prioritize it accordingly.
        private void CheckFor(CarWaypoint Checking, CarWaypoint Current, CarWaypoint Goal)
        {
            TheQueue.Enqueue(Checking, heuristic(Checking, Goal, CostSoFar[Current]));
            //Save cost so far and where it came from.
            //Did we arrive at this waypoint before?
            if (CameFrom.ContainsKey(Checking)) //Yes?
            {
                //Check at the cost
                if (CostSoFar[Checking] > CostSoFar[Current] + 1)
                {
                    //If current cost is better, replace old data with current one
                    CameFrom[Checking] = Current;
                    CostSoFar[Checking] = CostSoFar[Current] + 1;
                }
            }
            else //If not, just write the values down
            {
                CameFrom.Add(Checking, Current);
                CostSoFar.Add(Checking, CostSoFar[Current] + 1);
            }
        }

A few images to help you understand the structure

Waypoints structure over 3D world

195180-waypointscr.png Waypoint class structure

Apparently in my code, I enqueued the ones I enqueued before. That sometimes put my code into an endless loop. To fix this, I fixed my CheckFor method accordingly:

private void CheckFor(CarWaypoint Checking, CarWaypoint Current, CarWaypoint Goal)
{
    //Save cost so far and where it came from.
    //Did we arrive at this waypoint before?
    if (CameFrom.ContainsKey(Checking)) //Yes?
    {
         //Check at the cost
         if (CostSoFar[Checking] > CostSoFar[Current] + 1)
         {
             //If current cost is better, replace old data with current one
             CameFrom[Checking] = Current;
             CostSoFar[Checking] = CostSoFar[Current] + 1;
          }
    }
    else //If not, just write the values down
    {
        CameFrom.Add(Checking, Current);
        CostSoFar.Add(Checking, CostSoFar[Current] + 1);
        TheQueue.Enqueue(Checking, heuristic(Checking, Goal, CostSoFar[Current]));
     }
}