//Find a path between two positions
public void SeekPath(Vector3 startPosition, Vector3 endPosition, Action<IEnumerable<Vector3>> onComplete, IAmIntelligent intelligence = null)
{
//Scan if we don't have a map yet
if(cells == null)
Scan();
onComplete = onComplete ?? delegate {};
//Start and end in grid coordinates
var start = GetGridPosition(startPosition);
var end = GetGridPosition(endPosition);
endPosition.y = GetWorldPosition(end).y;
//Run multithreaded
Loom.RunAsync(()=>{
try
{
//Set of considered nodes
var closedSet = new Dictionary<GridPosition, Node>();
//Set of all nodes processed (so we can rebuild the path)
var map = new Dictionary<GridPosition, Node>();
//Set of nodes yet to be considered
var openSet = new Dictionary<GridPosition, Node>();
//Create a node for the start
var startNode = new Node { f_score = end.Distance(start) };
//No cost
startNode.g_score = 0;
//Add to the map and opensets
map[start] = startNode;
openSet[start] = startNode;
//Get the time so we can abort if it takes too long
var currentTime = DateTime.Now;
//While we have nodes
while(openSet.Count > 0 )
{
//Check if we should abort
if((DateTime.Now - currentTime).TotalMilliseconds > maxSeekTime)
{
break;
}
//Get the best possible node
var best = openSet.Aggregate((c,n)=>c.Value.f_score < n.Value.f_score ? c : n);
//Remove it from the open set and add it to
//the closed set
openSet.Remove(best.Key);
closedSet[best.Key] = best.Value;
//Have we reached the target?
if(best.Key == end)
{
//Recreate the path
var path = new List<Vector3>();
var scan = best.Value;
//Add the actual end position
path.Add(endPosition);
//Scan backwards from the end of the path
//until scan.cameFrom is 0
while(scan != null && scan.cameFrom != GridPosition.zero)
{
//Add the current node to the START of the path
//thereby reversing the direction of the list
path.Insert(0, GetWorldPosition(scan.cameFrom));
//Get the next node
scan = map[scan.cameFrom];
}
//Update the caller
Loom.QueueOnMainThread(()=> onComplete(path));
return;
}
//Get all of the neighbours of the current cell
//that are walkable
foreach(var cell in GetNeighbours(best.Key).Where(c=>GetCell(c).walkable))
{
//Have we processed this already?
if(closedSet.ContainsKey(cell))
{
continue;
}
//Work out the cost to the neighbour via the current node
var tentativeGScore = best.Value.g_score + GetWeightOfMovingBetween(best.Key, cell);
if(intelligence != null)
tentativeGScore += intelligence.GetWeight(best.Key, cell);
Node currentNode;
//Is the neighbour already open?
if(!openSet.TryGetValue(cell, out currentNode))
{
//If not then create a node for it
//this will have a maximum g_score
currentNode = new Node();
//Add it to the map and the open set
map[cell] = currentNode;
openSet[cell] = currentNode;
}
//Is the new g_score lower than the
//current one?
if(currentNode.g_score > tentativeGScore)
{
//Update the openset node with this
//new better way of getting there
currentNode.g_score = tentativeGScore;
currentNode.cameFrom = best.Key;
currentNode.f_score = tentativeGScore + cell.Distance(end);
}
}
}
//Indicate failure
Loom.QueueOnMainThread(()=> onComplete(null));
}
catch(Exception e)
{
Debug.Log(e.ToString());
}
});
}
//Find a path between two positions
public List <Vector3> SeekPath(Vector3 startPosition, Vector3 endPosition, IAmIntelligent intelligence = null)
{
//Scan if we don't have a map yet (did'nt scan yet)
if (cells == null)
{
Scan();
}
//Start and end in grid coordinates
var start = GetGridPosition(startPosition);
var end = GetGridPosition(endPosition);
endPosition.y = GetWorldPosition(end).y;
//Set of considered nodes
var closedSet = new Dictionary <GridPosition, Node>();
//Set of all nodes processed (so we can rebuild the path)
var map = new Dictionary <GridPosition, Node>();
//Set of nodes yet to be considered
var openSet = new Dictionary <GridPosition, Node>();
//Set the f and g score for the start node
//Create a node for the start
var startNode = new Node {
f_score = end.Distance(start)
};
//No cost
startNode.g_score = 0;
//Add the start node to the map and opensets
map[start] = startNode;
openSet[start] = startNode;
//While we have nodes in our openSet
while (openSet.Count > 0)
{
//Get the best possible node
var best = openSet.Aggregate((c, n) => c.Value.f_score < n.Value.f_score ? c : n);
//Remove it from the open set and add it to the closed set
openSet.Remove(best.Key);
closedSet[best.Key] = best.Value;
//Have we reached the target? if yes, build route
if (best.Key == end)
{
//Recreate the path
var path = new List <Vector3>();
var scan = best.Value;
//Add the actual end position
path.Add(endPosition);
//Scan backwards from the end of the path
//until scan.cameFrom is 0
while (scan != null && scan.cameFrom != GridPosition.zero)
{
//Add the current node to the START of the path thereby reversing the direction of the list
path.Insert(0, GetWorldPosition(scan.cameFrom));
//Get the next node
scan = map[scan.cameFrom];
}
//Update the caller
return(path);
}
//Get all of the neighbours of the current cell that are walkable
foreach (var cell in GetNeighbours(best.Key).Where(c => GetCell(c).walkable))
{
//Have we processed this already?
if (closedSet.ContainsKey(cell))
{
continue;
}
//Work out the cost to the neighbour via the current node
var tentativeGScore = best.Value.g_score + GetWeightOfMovingBetween(best.Key, cell);
if (intelligence != null)
{
tentativeGScore += intelligence.GetWeight(best.Key, cell);
}
Node currentNode;
//Is the neighbour already open?
if (!openSet.TryGetValue(cell, out currentNode))
{
//If not then create a node for it
//this will have a maximum g_score
currentNode = new Node();
//Add it to the map and the open set
map[cell] = currentNode;
openSet[cell] = currentNode;
}
//Is the new g_score lower than the
//current one?
if (currentNode.g_score > tentativeGScore)
{
//Update the openset node with this
//new better way of getting there
currentNode.g_score = tentativeGScore;
currentNode.cameFrom = best.Key;
currentNode.f_score = tentativeGScore + cell.Distance(end);
}
}
}
return(null);
}
//Find a path between two positions
public List<Vector3> SeekPath(Vector3 startPosition, Vector3 endPosition, IAmIntelligent intelligence = null)
{
//Scan if we don't have a map yet (did'nt scan yet)
if(cells == null)
Scan();
//Start and end in grid coordinates
var start = GetGridPosition(startPosition);
var end = GetGridPosition(endPosition);
endPosition.y = GetWorldPosition(end).y;
//Set of considered nodes
var closedSet = new Dictionary<GridPosition, Node>();
//Set of all nodes processed (so we can rebuild the path)
var map = new Dictionary<GridPosition, Node>();
//Set of nodes yet to be considered
var openSet = new Dictionary<GridPosition, Node>();
//Set the f and g score for the start node
//Create a node for the start
var startNode = new Node { f_score = end.Distance(start) };
//No cost
startNode.g_score = 0;
//Add the start node to the map and opensets
map[start] = startNode;
openSet[start] = startNode;
//While we have nodes in our openSet
while(openSet.Count > 0 )
{
//Get the best possible node
var best = openSet.Aggregate((c,n)=>c.Value.f_score < n.Value.f_score ? c : n);
//Remove it from the open set and add it to the closed set
openSet.Remove(best.Key);
closedSet[best.Key] = best.Value;
//Have we reached the target? if yes, build route
if(best.Key == end)
{
//Recreate the path
var path = new List<Vector3>();
var scan = best.Value;
//Add the actual end position
path.Add(endPosition);
//Scan backwards from the end of the path
//until scan.cameFrom is 0
while(scan != null && scan.cameFrom != GridPosition.zero)
{
//Add the current node to the START of the path thereby reversing the direction of the list
path.Insert(0, GetWorldPosition(scan.cameFrom));
//Get the next node
scan = map[scan.cameFrom];
}
//Update the caller
return path;
}
//Get all of the neighbours of the current cell that are walkable
foreach(var cell in GetNeighbours(best.Key).Where(c=>GetCell(c).walkable))
{
//Have we processed this already?
if(closedSet.ContainsKey(cell))
{
continue;
}
//Work out the cost to the neighbour via the current node
var tentativeGScore = best.Value.g_score + GetWeightOfMovingBetween(best.Key, cell);
if(intelligence != null)
tentativeGScore += intelligence.GetWeight(best.Key, cell);
Node currentNode;
//Is the neighbour already open?
if(!openSet.TryGetValue(cell, out currentNode))
{
//If not then create a node for it
//this will have a maximum g_score
currentNode = new Node();
//Add it to the map and the open set
map[cell] = currentNode;
openSet[cell] = currentNode;
}
//Is the new g_score lower than the
//current one?
if(currentNode.g_score > tentativeGScore)
{
//Update the openset node with this
//new better way of getting there
currentNode.g_score = tentativeGScore;
currentNode.cameFrom = best.Key;
currentNode.f_score = tentativeGScore + cell.Distance(end);
}
}
}
return null;
}
