Ejemplo n.º 1
0
        public static bool FindPath(BasePathNode start, BasePathNode goal, out Stack <BasePathNode> path, out float pathCost)
        {
            path     = null;
            pathCost = -1f;
            bool flag = false;

            if (start == goal)
            {
                return(false);
            }
            AStarNodeList          aStarNodeList = new AStarNodeList();
            HashSet <BasePathNode> basePathNodes = new HashSet <BasePathNode>();
            AStarNode aStarNode = new AStarNode(0f, AStarPath.Heuristic(start, goal), null, start);

            aStarNodeList.Add(aStarNode);
            while (aStarNodeList.Count > 0)
            {
                AStarNode item = aStarNodeList[0];
                aStarNodeList.RemoveAt(0);
                basePathNodes.Add(item.Node);
                if (!item.Satisfies(goal))
                {
                    foreach (BasePathNode node in item.Node.linked)
                    {
                        if (basePathNodes.Contains(node))
                        {
                            continue;
                        }
                        float     g           = item.G + AStarPath.Heuristic(item.Node, node);
                        AStarNode aStarNodeOf = aStarNodeList.GetAStarNodeOf(node);
                        if (aStarNodeOf != null)
                        {
                            if (g >= aStarNodeOf.G)
                            {
                                continue;
                            }
                            aStarNodeOf.Update(g, aStarNodeOf.H, item, node);
                            aStarNodeList.AStarNodeSort();
                        }
                        else
                        {
                            aStarNodeOf = new AStarNode(g, AStarPath.Heuristic(node, goal), item, node);
                            aStarNodeList.Add(aStarNodeOf);
                            aStarNodeList.AStarNodeSort();
                        }
                    }
                }
                else
                {
                    path     = new Stack <BasePathNode>();
                    pathCost = 0f;
                    while (item.Parent != null)
                    {
                        pathCost += item.F;
                        path.Push(item.Node);
                        item = item.Parent;
                    }
                    if (item != null)
                    {
                        path.Push(item.Node);
                    }
                    flag = true;
                    break;
                }
            }
            return(flag);
        }
Ejemplo n.º 2
0
        public static bool FindPath(
            BasePathNode start,
            BasePathNode goal,
            out Stack <BasePathNode> path,
            out float pathCost)
        {
            path     = (Stack <BasePathNode>)null;
            pathCost = -1f;
            bool flag = false;

            if (Object.op_Equality((Object)start, (Object)goal))
            {
                return(false);
            }
            AStarNodeList          astarNodeList   = new AStarNodeList();
            HashSet <BasePathNode> basePathNodeSet = new HashSet <BasePathNode>();
            AStarNode astarNode1 = new AStarNode(0.0f, AStarPath.Heuristic(start, goal), (AStarNode)null, start);

            astarNodeList.Add(astarNode1);
            while (astarNodeList.Count > 0)
            {
                AStarNode parent = astarNodeList[0];
                astarNodeList.RemoveAt(0);
                basePathNodeSet.Add(parent.Node);
                if (parent.Satisfies(goal))
                {
                    path     = new Stack <BasePathNode>();
                    pathCost = 0.0f;
                    for (; parent.Parent != null; parent = parent.Parent)
                    {
                        pathCost += parent.F;
                        path.Push(parent.Node);
                    }
                    if (parent != null)
                    {
                        path.Push(parent.Node);
                    }
                    flag = true;
                    break;
                }
                foreach (BasePathNode basePathNode in parent.Node.linked)
                {
                    if (!basePathNodeSet.Contains(basePathNode))
                    {
                        float     g           = parent.G + AStarPath.Heuristic(parent.Node, basePathNode);
                        AStarNode astarNodeOf = astarNodeList.GetAStarNodeOf(basePathNode);
                        if (astarNodeOf == null)
                        {
                            AStarNode astarNode2 = new AStarNode(g, AStarPath.Heuristic(basePathNode, goal), parent, basePathNode);
                            astarNodeList.Add(astarNode2);
                            astarNodeList.AStarNodeSort();
                        }
                        else if ((double)g < (double)astarNodeOf.G)
                        {
                            astarNodeOf.Update(g, astarNodeOf.H, parent, basePathNode);
                            astarNodeList.AStarNodeSort();
                        }
                    }
                }
            }
            return(flag);
        }