Ejemplo n.º 1
0
        public List <PathNode> FindPath(Double3 beginPoint, Double3 targetPoint)
        {
            Int3 beginBlock  = world.GetGound(beginPoint);
            Int3 targetBlock = world.GetGound(targetPoint);

            var startNode = new PathNode(beginBlock, 0, beginBlock.Distance(targetBlock), MoveActions.Walk);

            openList = new List <PathNode> {
                startNode
            };
            closedList = new List <Int3>();

            startNode.GCost = 0;
            startNode.HCost = CalculateDistanceCost(startNode.Position, targetBlock);

            while (openList.Count > 0 && Discoverd.Count < maxNodes)
            {
                PathNode currentNode = GetLowestFCostNode(openList);
                if (currentNode.Position == targetBlock)
                {
                    // Reached final node
                    return(CalculatePath(currentNode));
                }
                openList.Remove(currentNode);
                closedList.Add(currentNode.Position);

                foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
                {
                    if (closedList.Contains(neighbourNode.Position))
                    {
                        continue;
                    }
                    int tententiveGCos = currentNode.GCost + CalculateDistanceCost(currentNode.Position, neighbourNode.Position);
                    if (tententiveGCos < neighbourNode.GCost && tententiveGCos < maxEstimatePath)
                    {
                        neighbourNode.cameFromNode = currentNode;
                        neighbourNode.GCost        = tententiveGCos;
                        neighbourNode.HCost        = CalculateDistanceCost(neighbourNode.Position, targetBlock);
                        neighbourNode.CalculateFCost();

                        if (!openList.Contains(neighbourNode))
                        {
                            openList.Add(neighbourNode);
                        }
                    }
                }
            }

            return(null);
        }