Exemple #1
0
        public void AStarSearch(Vector2 sourcePoint, Vector2 destinationPoint, out Vector2 nextNode)
        {
            //Console.WriteLine("Going from (" + sourcePoint.X + ", " + sourcePoint.Y + ") to (" + destinationPoint.X + ", " + destinationPoint.Y + ")");
            Node source = adjacencyList[(int)(columns * sourcePoint.X + sourcePoint.Y)];
            Node destination = adjacencyList[(int)(columns * destinationPoint.X + destinationPoint.Y)];

            PriorityQueue <Node> Q = new PriorityQueue<Node>();
            source.Distance = 0;
            Q.Add(source.Priority, source);

            while (Q.Count > 0)
            {
                Node current = Q.RemoveMin();

                if (current == destination)
                {
                    Node pathParent = current.Parent;
                    Stack<Node> path = new Stack<Node>();
                    path.Push(current);
                    while (pathParent != null)
                    {
                        path.Push(pathParent);
                        pathParent = pathParent.Parent;
                    }

                    Thread t = new Thread(new ParameterizedThreadStart(DrawPath));
                    t.Start(path);

                    path.Pop();
                    nextNode = new Vector2(path.Peek().X * 32, path.Peek().Y * 32);

                    return;
                }

                List<Node> allAdjacent = new List<Node>();
                allAdjacent.AddRange(current.AdjacentNodes);
                allAdjacent.AddRange(current.CornerNodes);
                foreach (Node n in allAdjacent)
                {
                    Node node = adjacencyList[columns * n.X + n.Y];
                    if (node.Distance == int.MaxValue)
                    {
                        node.Distance = current.Distance + 1;
                        node.Parent = current;
                        node.Visited = true;
                        node.Priority = (int)(destinationPoint - sourcePoint).Length() + node.Distance;
                        Q.Add(node.Priority, node);
                    }
                }
            }
            nextNode = new Vector2(-1);
        }
Exemple #2
0
        public static Path findPath(Node startNode, Node destination)
        {
            List<Node> closedList = new List<Node>();
            PriorityQueue<Node> openList = new PriorityQueue<Node>();

            Dictionary<Node, int> gScore = new Dictionary<Node,int>();
            gScore[startNode] = 0;

            Dictionary<Node, int> fScore = new Dictionary<Node,int>();
            fScore[startNode] = startNode.HeuristicDistance(destination);

            Dictionary<Node, Node> prevNode = new Dictionary<Node, Node>();

            openList.Add(fScore[startNode], startNode);

            while (openList.Count > 0)
            {
                Node current = openList.RemoveMin();
                if (current.Equals(destination))
                {
                    return getPath(prevNode, destination);
                }
                else
                {
                    closedList.Add(current);
                    Node[] neighbours = current.GetNeighbours();
                    foreach (Node next in neighbours)
                    {
                        if (closedList.Contains(next))
                            continue;

                        int newGScore = gScore[current] + current.distanceBetween(next);

                        if (!openList.Contains(next) || newGScore < gScore[next])
                        {
                            prevNode[next] = current;
                            gScore[next] = newGScore;
                            fScore[next] = gScore[next] + next.HeuristicDistance(destination);
                            if (!openList.Contains(next))
                                openList.Add(fScore[next], next);
                        }
                    }
                }
            }

            return null;
        }