private void AddNode(Vector3 position, Vector3 target, Node parent, BaseSoldier unit)
{
int address = GridAux.PositionToId(position);
#if (FOUNDATION_DEBUG_PATHFINDER)
Log.Debug("PathFinder", "address={0}, position={1}", address, position);
#endif
if (!_closedSet.Contains(address))
{
BaseTile t = _gridTiles.TryGetTile(position);
if (t != null && unit.CanWalkPast(t))
{
int cost = (parent == null ? 0 : parent.Cost) + t.GetCost(unit);
int heuristic = GetManhattanDistance(position, target);
#if (FOUNDATION_DEBUG_PATHFINDER)
Log.Debug("PathFinder", "adding node. (position={0}, heuristic={1}, cost={2})", position, heuristic, cost);
#endif
_priorityQueue.Enqueue(new Node(position, parent, cost, heuristic));
}
else
{
Close(position);
}
}
}
