Esempio n. 1
0
            private static INode[] GetNeighbors(IMapData Map, NavigationNode Node)
            {
                List <INode> result = new List <INode>();

                if (Node.Parent == null)
                {
                    INode[] nodes = Map.GetAdjucentNodes(Node.Node);
                    for (int i = 0; i < nodes.Length; ++i)
                    {
                        result.Add(nodes[i]);
                    }
                }
                else
                {
                    Vector3 origin = Node.Node.Position;
                    Vector3 diff   = origin - Node.Parent.Node.Position;
                    int     dX     = (int)(diff.X / Math.Max(Math.Abs(diff.X), 1));
                    int     dY     = (int)(diff.Z / Math.Max(Math.Abs(diff.Z), 1));

                    INode tempNode = null;

                    if (dX != 0 && dY != 0)
                    {
                        tempNode = GetNode(Map, Node.Node, 0, dY);
                        if (Helper.IsWalkable(tempNode))
                        {
                            result.Add(tempNode);
                        }

                        tempNode = GetNode(Map, Node.Node, dX, 0);
                        if (Helper.IsWalkable(tempNode))
                        {
                            result.Add(tempNode);
                        }

                        tempNode = GetNode(Map, Node.Node, dX, dY);
                        if (Helper.IsWalkable(tempNode))
                        {
                            result.Add(tempNode);
                        }

                        tempNode = GetNode(Map, Node.Node, -dX, 0);
                        if (!Helper.IsWalkable(tempNode))
                        {
                            tempNode = GetNode(Map, Node.Node, -dX, dY);
                            if (tempNode != null)
                            {
                                result.Add(tempNode);
                            }
                        }

                        tempNode = GetNode(Map, Node.Node, 0, -dY);
                        if (!Helper.IsWalkable(tempNode))
                        {
                            tempNode = GetNode(Map, Node.Node, dX, -dY);
                            if (tempNode != null)
                            {
                                result.Add(tempNode);
                            }
                        }
                    }
                    else if (dX == 0)
                    {
                        tempNode = GetNode(Map, Node.Node, 0, dY);
                        if (Helper.IsWalkable(tempNode))
                        {
                            result.Add(tempNode);
                        }

                        tempNode = GetNode(Map, Node.Node, 1, 0);
                        if (!Helper.IsWalkable(tempNode))
                        {
                            tempNode = GetNode(Map, Node.Node, 1, dY);
                            if (tempNode != null)
                            {
                                result.Add(tempNode);
                            }
                        }

                        tempNode = GetNode(Map, Node.Node, -1, 0);
                        if (!Helper.IsWalkable(tempNode))
                        {
                            tempNode = GetNode(Map, Node.Node, -1, dY);
                            if (tempNode != null)
                            {
                                result.Add(tempNode);
                            }
                        }
                    }
                    else if (dY == 0)
                    {
                        tempNode = GetNode(Map, Node.Node, dX, 0);
                        if (Helper.IsWalkable(tempNode))
                        {
                            result.Add(tempNode);
                        }

                        tempNode = GetNode(Map, Node.Node, 0, 1);
                        if (!Helper.IsWalkable(tempNode))
                        {
                            tempNode = GetNode(Map, Node.Node, dX, 1);
                            if (tempNode != null)
                            {
                                result.Add(tempNode);
                            }
                        }

                        tempNode = GetNode(Map, Node.Node, 0, -1);
                        if (!Helper.IsWalkable(tempNode))
                        {
                            tempNode = GetNode(Map, Node.Node, dX, -1);
                            if (tempNode != null)
                            {
                                result.Add(tempNode);
                            }
                        }
                    }
                }

                return(result.ToArray());
            }
Esempio n. 2
0
            private static Vector3[] CalculatePath(IMapData Map, INode StartNode, INode GoalNode)
            {
                //1. Add the starting node to the open list.
                //2. Repeat the following steps:
                //	a. Look for the node which has the lowest f on the open list. Refer to this node as the current node.
                //	b. Switch it to the closed list.
                //	c. For each reachable node from the current node
                //		i. If it is on the closed list, ignore it.
                //		ii. If it isn’t on the open list, add it to the open list. Make the current node the parent of this node. Record the f, g, and h value of this node.
                //		iii. If it is on the open list already, check to see if this is a better path. If so, change its parent to the current node, and recalculate the f and g value.
                //	d. Stop when
                //		i. Add the target node to the closed list.
                //		ii. Fail to find the target node, and the open list is empty.
                //3. Tracing backwards from the target node to the starting node. That is your path.


                if (Map == null)
                {
                    return(null);
                }
                if (StartNode == null)
                {
                    return(null);
                }
                if (GoalNode == null)
                {
                    return(null);
                }
                if (StartNode == GoalNode)
                {
                    return(null);
                }
                if (StartNode.State == NodeStates.Blocked || GoalNode.State == NodeStates.Blocked)
                {
                    return(null);
                }

                NavigationNodeList openNodes   = new NavigationNodeList();
                NavigationNodeList closedNodes = new NavigationNodeList();

                NavigationNode startNavNode = new NavigationNode(StartNode);

                openNodes.Add(startNavNode);

                NavigationNode currNode = null;

                bool goalFound = false;

                while (true)
                {
                    if (openNodes.Count == 0)
                    {
                        break;
                    }

                    currNode = GetLowestCost(openNodes);
                    closedNodes.Add(currNode);
                    openNodes.Remove(currNode);

                    if (currNode.Node == GoalNode)
                    {
                        goalFound = true;

                        break;
                    }

                    INode[] adjNodes = Map.GetAdjucentNodes(currNode.Node, NodeStates.Walkable);

                    for (int i = 0; i < adjNodes.Length; ++i)
                    {
                        INode adjNode = adjNodes[i];

                        if (adjNode.State == NodeStates.Blocked)
                        {
                            continue;
                        }

                        if (Contains(closedNodes, adjNode))
                        {
                            continue;
                        }

                        NavigationNode adjNavNode = GetNavigationNode(openNodes, adjNode);
                        if (adjNavNode == null)
                        {
                            adjNavNode = new NavigationNode(adjNode);
                            openNodes.Add(adjNavNode);
                        }
                        else if (adjNavNode.GetTotalCost(GoalNode) >= currNode.ExactCost)
                        {
                            continue;
                        }

                        adjNavNode.SetParent(currNode);
                        adjNavNode.CalculateHeuristicCost(GoalNode);
                        adjNavNode.CalculateTotalCost();
                    }
                }

                if (!goalFound)
                {
                    return(null);
                }

                return(Helper.GetPositions(Helper.GetOptimizedNodes(Map, (GetFullPositions(currNode)))));
            }