public AStar(IEnumerable<IntVector3> initialLocations, IAStarTarget target)
{
this.MaxNodeCount = 200000;
this.CancellationToken = CancellationToken.None;
m_target = target;
m_nodeMap = new Dictionary<IntVector3, AStarNode>();
m_openList = new BinaryHeap<AStarNode>();
foreach (var p in initialLocations)
{
ushort g = 0;
ushort h = m_target.GetHeuristic(p);
var node = new AStarNode(p, null);
node.G = g;
node.H = h;
m_openList.Add(node);
m_nodeMap.Add(p, node);
}
}
public AStarNode(IntVector3 l, AStarNode parent)
{
Loc = l;
Parent = parent;
}
void UpdateNodes(AStarNode parent, Stack<AStarNode> queue)
{
foreach (var dir in m_target.GetValidDirs(parent.Loc))
{
IntVector3 childLoc = parent.Loc + dir;
AStarNode child;
m_nodeMap.TryGetValue(childLoc, out child);
if (child == null)
continue;
ushort g = (ushort)(parent.G + m_target.GetCostBetween(parent.Loc, childLoc));
if (g < child.G)
{
//Debug.Print("closed node {0} updated 1", childLoc);
child.Parent = parent;
child.G = g;
queue.Push(child);
}
}
}
void UpdateParents(AStarNode parent)
{
//Debug.Print("updating closed node {0}", parent.Loc);
Stack<AStarNode> queue = new Stack<AStarNode>();
UpdateNodes(parent, queue);
while (queue.Count > 0)
{
parent = queue.Pop();
UpdateNodes(parent, queue);
}
}
void CheckNeighbors(AStarNode parent)
{
foreach (var dir in m_target.GetValidDirs(parent.Loc))
{
IntVector3 childLoc = parent.Loc + dir;
AStarNode child;
m_nodeMap.TryGetValue(childLoc, out child);
//if (child != null && child.Closed)
// continue;
ushort g = (ushort)(parent.G + m_target.GetCostBetween(parent.Loc, childLoc));
ushort h = m_target.GetHeuristic(childLoc);
if (child == null)
{
child = new AStarNode(childLoc, parent);
child.G = g;
child.H = h;
m_openList.Add(child);
m_nodeMap.Add(childLoc, child);
}
else if (child.G > g)
{
child.Parent = parent;
child.G = g;
if (child.Closed == false)
m_openList.NodeUpdated(child);
else // Closed == true
UpdateParents(child);
}
}
}
public IEnumerable<Direction> GetPathReverse(AStarNode lastNode)
{
if (lastNode == null)
yield break;
AStarNode n = lastNode;
while (n.Parent != null)
{
yield return (n.Parent.Loc - n.Loc).ToDirection();
n = n.Parent;
}
}
internal AStarResult(AStarStatus status, AStarNode lastNode)
{
this.LastNode = lastNode;
this.Status = status;
}
internal AStarResult(AStarStatus status, AStarNode lastNode)
{
this.LastNode = lastNode;
this.Status = status;
}
public AStarNode(IntVector3 l, AStarNode parent)
{
Loc = l;
Parent = parent;
}