MoodSwingGame.MSMap.GetPath C# (CSharp) Method

        public Node GetPath(Vector2 start, Vector2 end)
        {
            List<Node> toCheck = new List<Node>();
            List<Node> done = new List<Node>();

            bool[,] hasVis = new bool[rows, columns];

            toCheck.Add(new Node((int)start.X, (int)start.Y, 1, 0, null));
            hasVis[(int)start.X, (int)start.Y] = true;
            Node last = null;
            bool getRoadFirst = true;
            if (mapArray[(int)start.X, (int)start.Y] is MSRoad) getRoadFirst = false;

            while (toCheck.Count != 0)
            {
                toCheck.Sort();
                Node visiting = toCheck.ElementAt<Node>(0);
                toCheck.RemoveAt(0);
                done.Add(visiting);

                if (visiting.Position == end)
                {
                    last = visiting;
                    break;
                }
                else
                {
                    int x = (int)visiting.Position.X;
                    int y = (int)visiting.Position.Y;

                    //Check tile above.
                    if (y + 1 < columns && (mapArray[x, y + 1] is MSRoad ||
                        (!getRoadFirst && new Vector2(x, y + 1) == end)) )
                    {
                        if (hasVis[x, y + 1] == false)
                        {
                            hasVis[x, y + 1] = true;
                            toCheck.Add(new Node(x, y + 1, visiting.G + 1, (int)Math.Abs(x - end.X) + (int)Math.Abs(y + 1 - end.Y), visiting));
                        }
                        else
                        {
                            foreach (Node temp in toCheck)
                            {
                                if (temp.Position == new Vector2(x, y + 1))
                                {
                                    temp.G = visiting.G + 1;
                                    temp.parent = visiting;
                                }
                            }
                        }
                    }

                    //check tile below
                    if (y - 1 >= 0 && (mapArray[x, y - 1] is MSRoad ||
                        (!getRoadFirst && new Vector2(x, y - 1) == end)))
                    {
                        if (hasVis[x, y - 1] == false)
                        {
                            hasVis[x, y - 1] = true;
                            toCheck.Add(new Node(x, y - 1, visiting.G + 1, (int)Math.Abs(x - end.X) + (int)Math.Abs(y - 1 - end.Y), visiting));
                        }
                        else
                        {
                            foreach (Node temp in toCheck)
                            {
                                if (temp.Position == new Vector2(x, y - 1))
                                {
                                    temp.G = visiting.G + 1;
                                    temp.parent = visiting;
                                }
                            }
                        }
                    }

                    //check tile on the right
                    if (x + 1 < rows &&( mapArray[x + 1, y] is MSRoad ||
                        (!getRoadFirst && new Vector2(x + 1, y) == end)))
                    {
                        if (hasVis[x + 1, y] == false)
                        {
                            hasVis[x + 1, y] = true;
                            toCheck.Add(new Node(x + 1, y, visiting.G + 1, (int)Math.Abs(x + 1 - end.X) + (int)Math.Abs(y - end.Y), visiting));
                        }
                        else
                        {
                            foreach (Node temp in toCheck)
                            {
                                if (temp.Position == new Vector2(x + 1, y))
                                {
                                    temp.G = visiting.G + 1;
                                    temp.parent = visiting;
                                }
                            }
                        }
                    }

                    //check tile on the left
                    if (x - 1 >= 0 && (mapArray[x - 1, y] is MSRoad ||
                        (!getRoadFirst && new Vector2(x - 1, y) == end)))
                    {
                        if (hasVis[x - 1, y] == false)
                        {
                            hasVis[x - 1, y] = true;
                            toCheck.Add(new Node(x - 1, y, visiting.G + 1, (int)Math.Abs(x - 1 - end.X) + (int)Math.Abs(y - end.Y), visiting));
                        }
                        else
                        {
                            foreach (Node temp in toCheck)
                            {
                                if (temp.Position == new Vector2(x - 1, y))
                                {
                                    temp.G = visiting.G + 1;
                                    temp.parent = visiting;
                                }
                            }
                        }
                    }
                    if (getRoadFirst == true) getRoadFirst = false;
                }
            }

            if (last != null)
            {
                while (last.parent != null)
                {
                    Node par = last.parent;
                    par.next = last;
                    last = par;
                }
            }

            return last;
        }

 public void VolunteerCitizen(MSMap map)
 {
     MS3DTile bldg = map.GetRandomCitizenSource();
     MS3DTile center = map.GetNearestVolunteerCenter(bldg);
     Node path = map.GetPath(bldg.TileCoordinate, center.TileCoordinate);
     MSVolunteeringCitizen v = new MSVolunteeringCitizen(bldg.Position + MSUnit.UNITZ_POSITION, path, map, 0);
     units.Add(v);
 }