private static NavigationNode GetOrAddNavigationNode(INode Node, NavigationNodeStack Nodes)
{
NavigationNode navNode = GetNavigationNode(Node, Nodes);
if (navNode != null)
{
return(navNode);
}
navNode = new NavigationNode(Node);
Nodes.Enqueue(navNode);
return(navNode);
}
private static Vector3[] CalculatePath(IMapData Map, INode StartNode, INode GoalNode)
{
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);
}
NavigationNodeStack openedNodes = new NavigationNodeStack();
NavigationNodeStack closedNodes = new NavigationNodeStack();
GetOrAddNavigationNode(StartNode, openedNodes);
NavigationNode goalNode = null;
while (openedNodes.Count != 0)
{
NavigationNode node = openedNodes.Dequeue();
closedNodes.Enqueue(node);
if (node.Node == GoalNode)
{
goalNode = node;
break;
}
IdentifySuccessors(Map, node, GoalNode, openedNodes, closedNodes);
}
if (goalNode == null)
{
return(null);
}
return(Helper.GetPositions(Helper.GetOptimizedNodes(Map, GetFullPositions(goalNode))));
}
private static NavigationNode GetNavigationNode(INode Node, NavigationNodeStack Nodes)
{
var it = Nodes.GetEnumerator();
while (it.MoveNext())
{
if (it.Current.Node == Node)
{
return(it.Current);
}
}
return(null);
}
private static bool IsNavigationNodeInList(INode Node, NavigationNodeStack Nodes)
{
return(GetNavigationNode(Node, Nodes) != null);
}
private static INode Jump(IMapData Map, INode Node, INode Parent, INode Goal, NavigationNodeStack OpenedNodes)
{
if (!Helper.IsWalkable(Node))
{
return(null);
}
if (Node == Goal)
{
return(Node);
}
Vector3 diff = Node.Position - Parent.Position;
if (diff.X != 0 && diff.Z != 0)
{
if ((Helper.IsWalkable(GetNode(Map, Node, diff)) && !Helper.IsWalkable(GetNode(Map, Node, diff.X * -1, 0))) ||
(Helper.IsWalkable(GetNode(Map, Node, diff * -1)) && !Helper.IsWalkable(GetNode(Map, Node, 0, diff.Z * -1))))
{
return(Node);
}
if (Jump(Map, GetNode(Map, Node, diff.X, 0), Node, Goal, OpenedNodes) != null || Jump(Map, GetNode(Map, Node, 0, diff.Z), Node, Goal, OpenedNodes) != null)
{
return(Node);
}
}
else if (diff.X == 0)
{
if ((Helper.IsWalkable(GetNode(Map, Node, 1, diff.Z)) && !Helper.IsWalkable(GetNode(Map, Node, 1, 0))) ||
(Helper.IsWalkable(GetNode(Map, Node, -1, diff.Z)) && !Helper.IsWalkable(GetNode(Map, Node, -1, 0))))
{
return(Node);
}
}
else if (diff.Z == 0)
{
if ((Helper.IsWalkable(GetNode(Map, Node, diff.X, 1)) && !Helper.IsWalkable(GetNode(Map, Node, 0, 1))) ||
(Helper.IsWalkable(GetNode(Map, Node, diff.X, -1)) && !Helper.IsWalkable(GetNode(Map, Node, 0, -1))))
{
return(Node);
}
}
return(Jump(Map, GetNode(Map, Node, diff), Node, Goal, OpenedNodes));
}
private static void IdentifySuccessors(IMapData Map, NavigationNode Node, INode Goal, NavigationNodeStack OpenedNodes, NavigationNodeStack ClosedNodes)
{
INode[] neighbors = GetNeighbors(Map, Node);
for (int i = 0; i < neighbors.Length; ++i)
{
INode neighborNode = neighbors[i];
INode jumpNode = Jump(Map, neighborNode, Node.Node, Goal, OpenedNodes);
if (jumpNode == null || IsNavigationNodeInList(jumpNode, ClosedNodes))
{
continue;
}
NavigationNode jumpNavNode = GetNavigationNode(jumpNode, OpenedNodes);
bool alreadyIsInOpenNodes = (jumpNavNode != null);
if (jumpNavNode == null)
{
jumpNavNode = GetOrAddNavigationNode(jumpNode, OpenedNodes);
}
Number heuristic = NavigationNode.CalculateHeuristicCost(Node.Node, jumpNode);
Number nextExactCost = Node.ExactCost + heuristic;
if (!alreadyIsInOpenNodes || nextExactCost < jumpNavNode.ExactCost)
{
jumpNavNode.SetExactCost(nextExactCost);
jumpNavNode.SetHeuristicCost(jumpNavNode.HeuristicCost != 0 ? jumpNavNode.HeuristicCost : NavigationNode.CalculateHeuristicCost(jumpNode, Goal));
jumpNavNode.SetParent(Node);
}
}
}
