AStarCollisionMap.Pathfinding.AStar.FindPath C# (CSharp) Метод

        public CustomArrayList<PathfindingNode> FindPath()
        {
            if (start == null || end == null)
            {
                return null;
            }
            openList.AddLast(start);

            while (openList.Count() != 0 )
            {
                PathfindingNode node = null;
                for( int i = 0; i < openList.Count(); i++ ){
                    PathfindingNode openNode = openList.ElementAt(i);
                    if (node == null || node.score > openNode.score) node = openNode;
                }
                if (node == end) return node.BuildPath(start);

                openList.Remove(node, true);
                closedList.AddLast(node);

                CustomArrayList<PathfindingNode> connectedNodes = node.GetConnectedNodes();
                for( int i = 0; i < connectedNodes.Count(); i++ ){
                    PathfindingNode currentSuccessor = connectedNodes.ElementAt(i);

                    if( closedList.Contains( currentSuccessor ) ) continue;
                    int currentClosestDistance = node.costToStart + (int)PathfindingUtil.GetHypoteneuseLength(currentSuccessor.GetLocation(), node.GetLocation());
                    Boolean better = false;

                    if( !openList.Contains( currentSuccessor ) ){
                        openList.AddLast(currentSuccessor);
                        better = true;
                    } else if( currentClosestDistance < currentSuccessor.costToStart ){
                        better = true;
                    }

                    if( better ) {
                        currentSuccessor.parent = node;
                        currentSuccessor.costToStart = currentClosestDistance;
                        currentSuccessor.costToEnd = currentSuccessor.CalculateScore(end.GetLocation());
                        currentSuccessor.score = currentSuccessor.costToStart + currentSuccessor.costToEnd;
                    }
                }
            }
            return null;
        }