예제 #1
0
파일: AStarSearch.cs 프로젝트: Hengle/AStar
        public IList <Node> Find(Node start, Node goal)
        {
            Start = start;
            Goal  = goal;

            Parent.Add(start, start);
            Open.Enqueue(Start, Heuristic(Start));

            while (Open.Count > 0)
            {
                Node node = Open.Dequeue();

                if (node.Id == Goal.Id)
                {
                    return(BuildPath());
                }

                AddUpdate(Closed, node.Id, node);

                foreach (ProposedStep step in Grid.Neighbours(node))
                {
                    Node neighour = step.Node;

                    if (!Closed.ContainsKey(neighour.Id))
                    {
                        if (!Open.Contains(neighour))
                        {
                            AddUpdate(GHistory, neighour.Id, double.PositiveInfinity);
                            TryRemove(Parent, neighour);
                        }

                        double gOld = TryGetValue(GHistory, neighour.Id);

                        // Compute Cost

                        if (UseLineOfSight && LineOfSight(TryGetValue(Parent, node), neighour))
                        {
                            var sParent     = TryGetValue(Parent, node);
                            var sParentCost = Cost(sParent, neighour);
                            if (TryGetValue(GHistory, sParent.Id) + sParentCost < TryGetValue(GHistory, neighour.Id))
                            {
                                AddUpdate(Parent, neighour, sParent);
                                AddUpdate(GHistory, neighour.Id, TryGetValue(GHistory, node.Id) + sParentCost);
                            }
                        }
                        else
                        {
                            if (TryGetValue(GHistory, node.Id) + step.Direction.Cost < TryGetValue(GHistory, neighour.Id))
                            {
                                AddUpdate(Parent, neighour, node);
                                AddUpdate(GHistory, neighour.Id, TryGetValue(GHistory, node.Id) + step.Direction.Cost);
                            }
                        }

                        // Compute Cost End

                        if (TryGetValue(GHistory, neighour.Id) < gOld)
                        {
                            if (Open.Contains(neighour))
                            {
                                Open.Remove(neighour);
                            }

                            Open.Enqueue(neighour, TryGetValue(GHistory, neighour.Id) + Heuristic(neighour));
                        }
                    }
                }
            }

            return(new List <Node>());
        }