コード例 #1
0
        public static FindPathResult <T> FindPath <T>(iExplorer <T> explorer, T start, List <T> goal)
            where T : PathNodeFPQN
        {
            var result = new FindPathResult <T>(c_MaxElementsInQueue, c_MaxElementsInQueue);

            if (start == null || goal == null || explorer == null)
            {
                return(result);
            }

            var closedSet = result.m_ClosedSet;
            var openSet   = result.m_OpenSet;

            start.PathLengthFromStart = 0.0f;
            start.PathLengthEstimated = implGetShortestPossiblePath(explorer, start, goal);
            openSet.Enqueue(start, 0);

            while (closedSet.Count < c_MaxElementsInQueue && openSet.Count > 0)
            {
                var currentNode = openSet.Dequeue();
                foreach (var n in goal)
                {
                    if (currentNode == n)
                    {
                        implGetPathForNode <T>(currentNode, result.m_Path);
                        return(result);
                    }
                }

                closedSet.Enqueue(currentNode, currentNode.PathLengthEstimated + currentNode.PathLengthFromStart);

                foreach (var neighbourNode in explorer.iGetNeighbours(currentNode as T))
                {
                    if (closedSet.Contains(neighbourNode))                                      // skip if already checked
                    {
                        continue;
                    }

                    var pathLengthFromStart = currentNode.PathLengthFromStart + explorer.iGetPathCost(currentNode as T, neighbourNode);
                    if (openSet.Contains(neighbourNode))
                    {                           // if presented and part is shorter then reset his parent and cost
                        if (neighbourNode.PathLengthFromStart > pathLengthFromStart)
                        {
                            neighbourNode.СameFrom            = currentNode;
                            neighbourNode.PathLengthFromStart = pathLengthFromStart;
                            openSet.UpdatePriority(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart);
                        }
                    }
                    else
                    {                           // if not presented add as wariant
                        neighbourNode.СameFrom            = currentNode;
                        neighbourNode.PathLengthFromStart = pathLengthFromStart;
                        neighbourNode.PathLengthEstimated = implGetShortestPossiblePath(explorer, neighbourNode as T, goal);
                        openSet.Enqueue(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart);
                    }
                }
            }

            return(null);
        }
コード例 #2
0
ファイル: AStar.cs プロジェクト: NullTale/Ludum-Dare-46-Game
        public static FindPathResult <T> FindPath <T>(this iExplorer <T> explorer, T start, T goal, CancellationToken cancellationToken, int maxChecks = c_MaxChecks, int bufferSize = c_BufferSize)
        {
            var result = new FindPathResult <T>();

            var startNode = new PathNode <T>(start);

            if (start == null || goal == null || explorer == null || explorer.iReachable(start, goal) == false)
            {
                return(result);
            }

            var closedSet = result.m_ClosedSet;
            var openSet   = new FastPriorityQueue <PathNode <T> >(bufferSize);

            result.m_OpenSet = openSet;

            startNode.СameFrom          = null;
            startNode.PathCost          = 0.0f;
            startNode.PathCostEstimated = explorer.iGetShortestPossiblePath(start, goal);
            startNode.Cost = 0.0f;
            openSet.Enqueue(startNode, startNode.Cost);

            // do while has variants
            while (openSet.Count > 0 && closedSet.Count < maxChecks && openSet.Count < bufferSize)
            {
                // cancellation check
                if (cancellationToken.IsCancellationRequested)
                {
                    return(result);
                }

                // get next node
                var currentNode = openSet.First();
                openSet.Remove(currentNode);

                // goal check
                if (currentNode.Master.Equals(goal))
                {
                    implGetPathForNode(currentNode, result.m_Path);
                    return(result);
                }

                // close current
                closedSet.Add(currentNode.Master);

                // proceed connections
                foreach (var neighborNode in explorer.iGetNeighbours(currentNode.Master))
                {
                    if (closedSet.Contains(neighborNode))                                       // skip if already checked
                    {
                        continue;
                    }

                    var pathCost = currentNode.PathCost + explorer.iGetPathCost(currentNode.Master, neighborNode);

                    // can use Dictionary instead FirstOrDefault
                    var openNode = openSet.FirstOrDefault(n => n.Master.Equals(neighborNode));
                    if (openNode != null)
                    {                           // if presented and part is shorter then reset his parent and cost
                        if (openNode.PathCost > pathCost)
                        {
                            openNode.СameFrom = currentNode;
                            openNode.PathCost = pathCost;
                            // update priority
                            openNode.Cost = openNode.PathCostEstimated + openNode.PathCost;

                            openSet.UpdatePriority(openNode, openNode.Cost);
                        }
                    }
                    else
                    {                           // if not presented add as variant
                        var pathNode = new PathNode <T>(neighborNode);
                        pathNode.СameFrom          = currentNode;
                        pathNode.PathCost          = pathCost;
                        pathNode.PathCostEstimated = explorer.iGetShortestPossiblePath(pathNode.Master, goal);
                        pathNode.Cost = pathNode.PathCostEstimated + pathNode.PathCost;
                        openSet.Enqueue(pathNode, pathNode.Cost);
                    }
                }
            }

            return(result);
        }