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());
}
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)))));
}