private static List <HierarchicalPathNode> GetConnectingNodes(HierarchicalPathNode currentNode, HierarchicalPathNode endNode)
{
List <HierarchicalPathNode> connectingNodes = new List <HierarchicalPathNode>();
foreach (HierarchicalNode node in currentNode.Node.ConnectingNodes.Keys)
{
connectingNodes.Add(new HierarchicalPathNode(currentNode, endNode, node, currentNode.Node.ConnectingNodes[node] + currentNode.DirectCost));
}
return(connectingNodes);
}
private static void AddNodeToOpenList(HierarchicalPathNode node)
{
int index = 0;
float cost = node.TotalCost;
while ((_openList.Count > index) &&
(cost < _openList[index].TotalCost))
{
index++;
}
_openList.Insert(index, node);
_nodeCosts[node] = node.TotalCost;
_nodeStatus[node] = NodeStatus.Open;
}
public HierarchicalPathNode(HierarchicalPathNode parentNode,
HierarchicalPathNode endNode,
HierarchicalNode node,
float cost)
{
ParentNode = parentNode;
EndNode = endNode;
Node = node;
DirectCost = cost;
if (endNode != null)
{
TotalCost = DirectCost + Distance(endNode);
}
}
//TODO return vectors instead
public static List <Vector3> FindPath(Graph level, Vector3 start, Vector3 end)
{
if (!level.GetNode(start).walkable || !level.GetNode(end).walkable)
{
return(null);
}
_openList.Clear();
_nodeStatus.Clear();
_nodeCosts.Clear();
HierarchicalPathNode startNode;
HierarchicalPathNode endNode;
endNode = new HierarchicalPathNode(null, null, level.GetNode(end), 0);
startNode = new HierarchicalPathNode(null, endNode, level.GetNode(start), 0);
if (InSight(start, end))
{
return(new List <Vector3>()
{
start, end
});
}
AddNodeToOpenList(startNode);
while (_openList.Count > 0)
{
HierarchicalPathNode currentNode = _openList[_openList.Count - 1];
if (currentNode.EqualsPathNode(endNode))
{
List <Vector3> bestPath = new List <Vector3>();
while (currentNode != null)
{
//Check with end pos, not end node
bestPath.Insert(0, currentNode.Node._graphPos);
/*while ((currentNode.ParentNode != null && currentNode.ParentNode.ParentNode != null) && InSight(currentNode.Node._graphPos, currentNode.ParentNode.ParentNode.Node._graphPos))
* {
* currentNode.ParentNode = currentNode.ParentNode.ParentNode;
* }*/
currentNode = currentNode.ParentNode;
}
//Check with start pos, not start node
return(bestPath);
}
_openList.Remove(currentNode);
_nodeCosts.Remove(currentNode);
foreach (HierarchicalPathNode possibleNode in GetConnectingNodes(currentNode, endNode))
{
if (_nodeStatus.ContainsKey(possibleNode))
{
if (_nodeStatus[possibleNode] ==
NodeStatus.Closed)
{
continue;
}
else
{
if (possibleNode.TotalCost >=
_nodeCosts[possibleNode])
{
continue;
}
}
}
AddNodeToOpenList(possibleNode);
}
_nodeStatus[currentNode] = NodeStatus.Closed;
}
return(null);
}
public bool EqualsPathNode(HierarchicalPathNode otherNode)
{
return(otherNode.Node == this.Node);
}
public float Distance(HierarchicalPathNode otherNode)
{
//TODO find distance
return(Vector3.Distance(this.Node._graphPos, otherNode.Node._graphPos));
}