예제 #1
0
        /// <summary>
        /// A* pathfinding algorithm.
        /// </summary>
        /// <param name="request">PathRequest object.</param>
        /// <param name="callback">Callback object.</param>
        public void FindPath(PathRequest request, Action <PathResult> callback)
        {
            Stopwatch sw = new Stopwatch();

            sw.Start();

            List <Node> waypoints   = new List <Node>();
            bool        pathSuccess = false;

            Node startNode  = _aGrid.getNode(request.pathStart);
            Node targetNode = _aGrid.getNode(request.pathEnd);

            startNode.parent = startNode;


            if (targetNode.GetState() == Enums.TileState.FREE)
            {
                Heap <Node>    openSet   = new Heap <Node>(_aGrid.MaxSize);
                HashSet <Node> closedSet = new HashSet <Node>();
                openSet.Add(startNode);

                while (openSet.Count > 0)
                {
                    Node currentNode = openSet.RemoveFirst();
                    closedSet.Add(currentNode);

                    if (currentNode == targetNode)
                    {
                        sw.Stop();
                        //print ("Path found: " + sw.ElapsedMilliseconds + " ms");
                        pathSuccess = true;
                        break;
                    }

                    foreach (Node neighbour in _aGrid.GetNeighbours(currentNode))
                    {
                        if (neighbour.GetState() != Enums.TileState.FREE || closedSet.Contains(neighbour))
                        {
                            continue;
                        }

                        int newMovementCostToNeighbour =
                            currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
                        if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
                        {
                            neighbour.gCost  = newMovementCostToNeighbour;
                            neighbour.hCost  = GetDistance(neighbour, targetNode);
                            neighbour.parent = currentNode;

                            if (!openSet.Contains(neighbour))
                            {
                                openSet.Add(neighbour);
                            }
                            else
                            {
                                openSet.UpdateItem(neighbour);
                            }
                        }
                    }
                }
            }

            if (pathSuccess)
            {
                waypoints   = RetracePath(startNode, targetNode);
                pathSuccess = waypoints.Count > 0;
            }

            callback(new PathResult(waypoints, pathSuccess, request.parameter, request.callback));
        }