예제 #1
0
        public static void Fill(Node position, int range, SuperNode supernode)
        {
            var openQueue   = new Utils.PriorityQueue <VisitedNode>();
            var pathNodeMap = new Dictionary <Node, VisitedNode>();

            pathNodeMap[position] = new VisitedNode(position, null, 0);
            openQueue.Enqueue(pathNodeMap[position], 0);
            while (!openQueue.IsEmpty())
            {
                var current = openQueue.Dequeue();
                current.GridNode.ConnectSuperNode(current.Prev != null ? current.Prev.GridNode : null, supernode, current.GScore);
                foreach (var neighbour in current.GridNode.GetNeighbours())
                {
                    if (!neighbour.To.SuperNodes.ContainsKey(supernode) && neighbour.Length + current.GScore <= range)
                    {
                        var newNode = new VisitedNode(neighbour.To, current, neighbour.Length);
                        if (pathNodeMap.ContainsKey(neighbour.To))
                        {
                            if (openQueue.Update(pathNodeMap[neighbour.To], pathNodeMap[neighbour.To].GScore, newNode, newNode.GScore))
                            {
                                pathNodeMap[neighbour.To] = newNode;
                            }
                        }
                        else
                        {
                            openQueue.Enqueue(newNode, newNode.GScore);
                            pathNodeMap[neighbour.To] = newNode;
                        }
                    }
                }
            }
        }
예제 #2
0
파일: AStar.cs 프로젝트: Genae/VoxelEngine
        public static Path GetPath(Dictionary <Node, float> from, List <Node> to, Path path)
        {
            var openSet     = new Utils.PriorityQueue <VisitedNode>();
            var pathNodeMap = new Dictionary <Node, VisitedNode>();
            var nodesTo     = new HashSet <VisitedNode>();

            foreach (var node in to)
            {
                nodesTo.Add(new VisitedNode(node, null, 0));
            }
            if (nodesTo.Count == 0)
            {
                return(null);
            }
            foreach (var f in from)
            {
                var nodeFrom = new VisitedNode(f.Key, null, f.Value);
                openSet.Enqueue(nodeFrom, nodeFrom.GetCost(nodesTo));
            }
            while (!openSet.IsEmpty())
            {
                var curNode = openSet.Dequeue();
                if (nodesTo.Contains(curNode))
                {
                    path = ReconstructPath(curNode, path);
                    //Debug.Log("Found path between " + nodeFrom.GridNode.Position + " and " + nodeTo.GridNode.Position + " of length: " + path.Length + " in " + (DateTime.Now-start).TotalMilliseconds + "ms.");
                    return(path);
                }
                curNode.Status = NodeStatus.Closed;
                foreach (var neighbour in curNode.GridNode.GetNeighbours())
                {
                    if (pathNodeMap.ContainsKey(neighbour.To))
                    {
                        var pathNode = pathNodeMap[neighbour.To];
                        if (pathNode.Status == NodeStatus.Closed)
                        {
                            continue;
                        }
                        var node = new VisitedNode(neighbour.To, curNode, neighbour.Length);
                        if (openSet.Update(pathNode, pathNode.GetCost(nodesTo), node, node.GetCost(nodesTo)))
                        {
                            pathNodeMap[neighbour.To] = node;
                        }
                    }
                    else
                    {
                        var node = new VisitedNode(neighbour.To, curNode, neighbour.Length);
                        openSet.Enqueue(node, node.GetCost(nodesTo));
                        pathNodeMap[neighbour.To] = node;
                    }
                }
            }
            //Debug.Log("Couldn't find path between " + nodeFrom.GridNode + " and " + nodeTo.GridNode + " in " + (DateTime.Now - start).TotalMilliseconds + "ms.");
            return(path);
        }
예제 #3
0
 public VisitedNode(Node node, VisitedNode prev, float cost)
 {
     GridNode = node;
     Prev     = prev;
     if (prev == null)
     {
         GScore = cost;
     }
     else
     {
         GScore = prev.GScore + cost;
     }
 }
예제 #4
0
파일: AStar.cs 프로젝트: Genae/VoxelEngine
        private static Path ReconstructPath(VisitedNode node, Path path)
        {
            var length = node.GScore;
            var nodes  = new List <Node>();

            while (node != null)
            {
                nodes.Add(node.GridNode);
                node = node.Prev;
            }
            nodes.Reverse();
            path.Nodes  = nodes;
            path.Length = length;
            return(path);
        }
예제 #5
0
        public static void FillNeigbours(SuperNode superNode, int gridSize)
        {
            var nodesToFill = superNode.GetNeighbours().Select(n => n.To).ToArray();
            var openQueue   = new Utils.PriorityQueue <VisitedNode>();
            var pathNodeMap = new Dictionary <Node, VisitedNode>();

            foreach (var childNode in superNode.ChildNodes)
            {
                if (childNode.SuperNodes[superNode].Length > gridSize * 0.8)
                {
                    pathNodeMap[childNode] = new VisitedNode(childNode, null, childNode.SuperNodes[superNode].Length);
                    openQueue.Enqueue(pathNodeMap[childNode], pathNodeMap[childNode].GScore);
                }
            }
            while (!openQueue.IsEmpty())
            {
                var current = openQueue.Dequeue();
                if (!current.GridNode.SuperNodes.ContainsKey(superNode))
                {
                    current.GridNode.ConnectSuperNode(current.Prev != null ? (current.Prev.GridNode ?? superNode) : superNode, superNode, current.GScore);
                }
                foreach (var neighbour in current.GridNode.GetNeighbours())
                {
                    if (!neighbour.To.SuperNodes.ContainsKey(superNode) && neighbour.To.SuperNodes.Any(k => k.Value.Length <= gridSize && nodesToFill.Contains(k.Key)))
                    {
                        var newNode = new VisitedNode(neighbour.To, current, neighbour.Length);
                        if (pathNodeMap.ContainsKey(neighbour.To))
                        {
                            if (openQueue.Update(pathNodeMap[neighbour.To], pathNodeMap[neighbour.To].GScore, newNode, newNode.GScore))
                            {
                                pathNodeMap[neighbour.To] = newNode;
                            }
                        }
                        else
                        {
                            openQueue.Enqueue(newNode, newNode.GScore);
                            pathNodeMap[neighbour.To] = newNode;
                        }
                    }
                }
            }
        }
예제 #6
0
 public bool Equals(VisitedNode node)
 {
     return(node.GridNode.Equals(GridNode));
 }