/// <summary> This returns a list of nodes around the given node, using the notion of "movement costs" to determine
/// if a node should be included in the returned list or not. The callback can be used to specify the "cost" to
/// reach the specified node, making this useful to select the nodes that a unit might be able to move to. </summary>
/// <param name="nodeIdx"> The central node's index. </param>
/// <param name="radius"> The maximum area around the node to select nodes from. </param>
/// <param name="callback"> An optional callback (pass null to not use it) that can be used to indicate the cost of
/// moving from one node to another. By default it will "cost" 1 to move from one node to another. By returning 0 in
/// this callback you can indicate that the target node can't be moved to (for example when the tile is occupied).
/// Return a value higher than one (like 2 or 3) if moving to the target node would cost more and potentially
/// exclude the node from the returned list of nodes (when cost to reach it would be bigger than "radius"). </param>
/// <returns> Returns a list of node indices that can be used with grid[]. Returns empty list (not null) if there was an error. </returns>
public virtual List <int> NodeIndicesAround(int nodeIdx, int radius, NodeCostCallback callback)
{
List <int> accepted = new List <int>(); // accepted nodes
Dictionary <int, float> costs = new Dictionary <int, float>(); // <idx, cost> - used to track which nodes have been checked
CheckNodesRecursive(nodeIdx, radius, callback, -1, 0, ref accepted, ref costs);
return(accepted);
}
/// <summary> This returns a list of nodes around the given node, using the notion of "movement costs" to determine
/// if a node should be included in the returned list or not. The callback can be used to specify the "cost" to
/// reach the specified node, making this useful to select the nodes that a unit might be able to move to. </summary>
/// <param name="node"> The central node. </param>
/// <param name="radius"> The maximum area around the node to select nodes from. </param>
/// <param name="callback"> An optional callback (pass null to not use it) that can be used to indicate the cost of
/// moving from one node to another. By default it will "cost" 1 to move from one node to another. By returning 0 in
/// this callback you can indicate that the target node can't be moved to (for example when the tile is occupied).
/// Return a value higher than one (like 2 or 3) if moving to the target node would cost more and potentially
/// exclude the node from the returned list of nodes (when cost to reach it would be bigger than "radius"). </param>
/// <returns> Returns a list of nodes that can be used with grid[]. Returns empty list (not null) if there was an error. </returns>
public virtual List <T> NodesAround <T>(MapNavNode node, int radius, NodeCostCallback callback)
where T : MapNavNode
{
List <int> accepted = NodeIndicesAround(node.idx, radius, callback);
if (accepted.Count > 0)
{
List <T> res = new List <T>();
for (int i = 0; i < accepted.Count; i++)
{
res.Add((T)grid[accepted[i]]);
}
return(res);
}
return(new List <T>(0));
}
/// <summary> This is a Helper for NodeIndicesAround(int idx, int radius, bool includeCentralNode, ValidationCallback callback) </summary>
protected virtual void CheckNodesRecursive(int idx, int radius, NodeCostCallback callback, int cameFrom, float currDepth, ref List <int> accepted, ref Dictionary <int, float> costs)
{
List <int> ids = _neighbours(idx);
for (int i = 0; i < ids.Count; i++)
{
// skip if came into this function from this node. no point in testing
// against the node that caused this one to be called for checking
if (cameFrom == ids[i])
{
continue;
}
// get cost to move to the node
float res = callback == null ? 1f : callback(grid[idx], grid[ids[i]]);
// can move to node?
if (res <= 0.0f)
{
continue;
}
// how much would it cost in total?
float d = currDepth + res;
// too much to reach node?
if (d > radius)
{
continue;
}
// this neighbour node can be moved to, add it to the accepted list if not yet present
if (false == accepted.Contains(ids[i]))
{
accepted.Add(ids[i]);
}
// do not bother to check the node's neighbours if already reached the max
if (d == radius)
{
continue;
}
// check if should look into the neighbours of this node
if (costs.ContainsKey(ids[i]))
{
// if the new total cost is higher than previously checked then skip this neighbour
// since testing with the higher costs will not change any results when checking
// the neighbours of this neighbour node
if (costs[ids[i]] <= d)
{
continue;
}
}
else
{
costs.Add(ids[i], d);
}
// update the cost to move to this node
costs[ids[i]] = d;
// and test its neighbours for possible valid nodes
CheckNodesRecursive(ids[i], radius, callback, idx, d, ref accepted, ref costs);
}
}
/// <summary> Returns a list of nodes that represents a path from one node to another. An A* algorithm is used
/// to calculate the path. Return an empty list on error or if the destination node can't be reached. </summary>
/// <param name="start"> The node where the path should start. </param>
/// <param name="end"> The node to reach. </param>
/// <param name="callback"> An optional callback that can return an integer value to indicate the
/// cost of moving onto the specified node. This callback should return 1
/// for normal nodes and 2+ for higher cost to move onto the node and 0
/// if the node can't be moved onto; for example when the node is occupied. </param>
/// <returns> Return an empty list on error or if the destination node can't be reached. </returns>
public virtual List <T> Path <T>(MapNavNode start, MapNavNode end, NodeCostCallback callback)
where T : MapNavNode
{
if (start == null || end == null)
{
return(new List <T>(0));
}
if (start.idx == end.idx)
{
return(new List <T>(0));
}
List <T> path = new List <T>();
int current = -1;
int next = -1;
float new_cost = 0;
float next_cost = 0;
double priority = 0;
// first check if not direct neighbour and get out early
List <int> neighbors = PathNodeIndicesAround(start.idx); // NodeIndicesAround(start.idx, false, false, null);
if (neighbors != null)
{
if (neighbors.Contains(end.idx))
{
if (callback != null)
{
next_cost = callback(start, end);
if (next_cost >= 1)
{
path.Add((T)end);
}
}
return(path);
}
}
HeapPriorityQueue <PriorityQueueNode> frontier = new HeapPriorityQueue <PriorityQueueNode>(grid.Length);
frontier.Enqueue(new PriorityQueueNode()
{
idx = start.idx
}, 0);
Dictionary <int, int> came_from = new Dictionary <int, int>(); // <for_idx, came_from_idx>
Dictionary <int, float> cost_so_far = new Dictionary <int, float>(); // <idx, cost>
came_from.Add(start.idx, -1);
cost_so_far.Add(start.idx, 0);
while (frontier.Count > 0)
{
current = frontier.Dequeue().idx;
if (current == end.idx)
{
break;
}
neighbors = PathNodeIndicesAround(current); //NodeIndicesAround(current, false, false, null);
if (neighbors != null)
{
for (int i = 0; i < neighbors.Count; i++)
{
next = neighbors[i];
if (callback != null)
{
next_cost = callback(grid[current], grid[next]);
}
if (next_cost <= 0.0f)
{
continue;
}
new_cost = cost_so_far[current] + next_cost;
if (false == cost_so_far.ContainsKey(next))
{
cost_so_far.Add(next, new_cost + 1);
}
if (new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
priority = new_cost + Heuristic(start.idx, end.idx, next);
frontier.Enqueue(new PriorityQueueNode()
{
idx = next
}, priority);
if (false == came_from.ContainsKey(next))
{
came_from.Add(next, current);
}
else
{
came_from[next] = current;
}
}
}
}
}
// build path
next = end.idx;
while (came_from.ContainsKey(next))
{
if (came_from[next] == -1)
{
break;
}
if (came_from[next] == start.idx)
{
break;
}
path.Add((T)grid[came_from[next]]);
next = came_from[next];
}
if (path.Count > 0)
{
path.Reverse();
path.Add((T)end);
}
return(path);
}
