public void givenTwoNodesThatAreEqual_assertEqual()
{
PathfindingNode one = new PathfindingNode(1.0f, 1.0f);
PathfindingNode two = new PathfindingNode(1.0f, 1.0f);
Assert.True(one.Equals(two));
}
protected override async Task <Queue <TileRef> > Process()
{
if (_startNode == null ||
_endNode == null ||
Status == JobStatus.Finished)
{
return(null);
}
// If we couldn't get a nearby node that's good enough
if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs))
{
return(null);
}
var openTiles = new PriorityQueue <ValueTuple <float, PathfindingNode> >(new PathfindingComparer());
var gScores = new Dictionary <PathfindingNode, float>();
var cameFrom = new Dictionary <PathfindingNode, PathfindingNode>();
var closedTiles = new HashSet <PathfindingNode>();
PathfindingNode currentNode = null;
openTiles.Add((0.0f, _startNode));
gScores[_startNode] = 0.0f;
var routeFound = false;
var count = 0;
while (openTiles.Count > 0)
{
count++;
if (count % 20 == 0 && count > 0)
{
await SuspendIfOutOfTime();
}
if (_startNode == null || _endNode == null)
{
return(null);
}
(_, currentNode) = openTiles.Take();
if (currentNode.Equals(_endNode))
{
routeFound = true;
break;
}
closedTiles.Add(currentNode);
foreach (var(direction, nextNode) in currentNode.Neighbors)
{
if (closedTiles.Contains(nextNode))
{
continue;
}
// If tile is untraversable it'll be null
var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode);
if (tileCost == null || !PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction))
{
continue;
}
var gScore = gScores[currentNode] + tileCost.Value;
if (gScores.TryGetValue(nextNode, out var nextValue) && gScore >= nextValue)
{
continue;
}
cameFrom[nextNode] = currentNode;
gScores[nextNode] = gScore;
// pFactor is tie-breaker where the fscore is otherwise equal.
// See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
// There's other ways to do it but future consideration
var fScore = gScores[nextNode] + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f);
openTiles.Add((fScore, nextNode));
}
}
if (!routeFound)
{
return(null);
}
var route = PathfindingHelpers.ReconstructPath(cameFrom, currentNode);
if (route.Count == 1)
{
return(null);
}
#if DEBUG
// Need to get data into an easier format to send to the relevant clients
if (DebugRoute != null && route.Count > 0)
{
var debugCameFrom = new Dictionary <TileRef, TileRef>(cameFrom.Count);
var debugGScores = new Dictionary <TileRef, float>(gScores.Count);
var debugClosedTiles = new HashSet <TileRef>(closedTiles.Count);
foreach (var(node, parent) in cameFrom)
{
debugCameFrom.Add(node.TileRef, parent.TileRef);
}
foreach (var(node, score) in gScores)
{
debugGScores.Add(node.TileRef, score);
}
foreach (var node in closedTiles)
{
debugClosedTiles.Add(node.TileRef);
}
var debugRoute = new SharedAiDebug.AStarRouteDebug(
_pathfindingArgs.Uid,
route,
debugCameFrom,
debugGScores,
debugClosedTiles,
DebugTime);
DebugRoute.Invoke(debugRoute);
}
#endif
return(route);
}
protected override async Task <Queue <TileRef> > Process()
{
// VERY similar to A*; main difference is with the neighbor tiles you look for jump nodes instead
if (_startNode == null ||
_endNode == null)
{
return(null);
}
// If we couldn't get a nearby node that's good enough
if (!Utils.TryEndNode(ref _endNode, _pathfindingArgs))
{
return(null);
}
var openTiles = new PriorityQueue <ValueTuple <float, PathfindingNode> >(new PathfindingComparer());
var gScores = new Dictionary <PathfindingNode, float>();
var cameFrom = new Dictionary <PathfindingNode, PathfindingNode>();
var closedTiles = new HashSet <PathfindingNode>();
#if DEBUG
var jumpNodes = new HashSet <PathfindingNode>();
#endif
PathfindingNode currentNode = null;
openTiles.Add((0, _startNode));
gScores[_startNode] = 0.0f;
var routeFound = false;
var count = 0;
while (openTiles.Count > 0)
{
count++;
// JPS probably getting a lot fewer nodes than A* is
if (count % 5 == 0 && count > 0)
{
await SuspendIfOutOfTime();
}
(_, currentNode) = openTiles.Take();
if (currentNode.Equals(_endNode))
{
routeFound = true;
break;
}
foreach (var(direction, _) in currentNode.Neighbors)
{
var jumpNode = GetJumpPoint(currentNode, direction, _endNode);
if (jumpNode != null && !closedTiles.Contains(jumpNode))
{
closedTiles.Add(jumpNode);
#if DEBUG
jumpNodes.Add(jumpNode);
#endif
// GetJumpPoint should already check if we can traverse to the node
var tileCost = Utils.GetTileCost(_pathfindingArgs, currentNode, jumpNode);
if (tileCost == null)
{
throw new InvalidOperationException();
}
var gScore = gScores[currentNode] + tileCost.Value;
if (gScores.TryGetValue(jumpNode, out var nextValue) && gScore >= nextValue)
{
continue;
}
cameFrom[jumpNode] = currentNode;
gScores[jumpNode] = gScore;
// pFactor is tie-breaker where the fscore is otherwise equal.
// See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
// There's other ways to do it but future consideration
var fScore = gScores[jumpNode] + Utils.OctileDistance(_endNode, jumpNode) * (1.0f + 1.0f / 1000.0f);
openTiles.Add((fScore, jumpNode));
}
}
}
if (!routeFound)
{
return(null);
}
var route = Utils.ReconstructJumpPath(cameFrom, currentNode);
if (route.Count == 1)
{
return(null);
}
#if DEBUG
// Need to get data into an easier format to send to the relevant clients
if (DebugRoute != null && route.Count > 0)
{
var debugJumpNodes = new HashSet <TileRef>(jumpNodes.Count);
foreach (var node in jumpNodes)
{
debugJumpNodes.Add(node.TileRef);
}
var debugRoute = new SharedAiDebug.JpsRouteDebug(
_pathfindingArgs.Uid,
route,
debugJumpNodes,
DebugTime);
DebugRoute.Invoke(debugRoute);
}
#endif
return(route);
}
protected override async Task <Queue <TileRef> > Process()
{
if (_startNode == null ||
_endNode == null ||
Status == JobStatus.Finished)
{
return(null);
}
// If we couldn't get a nearby node that's good enough
if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs))
{
return(null);
}
var frontier = new PriorityQueue <ValueTuple <float, PathfindingNode> >(new PathfindingComparer());
var costSoFar = new Dictionary <PathfindingNode, float>();
var cameFrom = new Dictionary <PathfindingNode, PathfindingNode>();
PathfindingNode currentNode = null;
frontier.Add((0.0f, _startNode));
costSoFar[_startNode] = 0.0f;
var routeFound = false;
var count = 0;
while (frontier.Count > 0)
{
// Handle whether we need to pause if we've taken too long
count++;
if (count % 20 == 0 && count > 0)
{
await SuspendIfOutOfTime();
if (_startNode == null || _endNode == null)
{
return(null);
}
}
// Actual pathfinding here
(_, currentNode) = frontier.Take();
if (currentNode.Equals(_endNode))
{
routeFound = true;
break;
}
foreach (var nextNode in currentNode.GetNeighbors())
{
// If tile is untraversable it'll be null
var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode);
if (tileCost == null)
{
continue;
}
// So if we're going NE then that means either N or E needs to be free to actually get there
var direction = PathfindingHelpers.RelativeDirection(nextNode, currentNode);
if (!PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction))
{
continue;
}
// f = g + h
// gScore is distance to the start node
// hScore is distance to the end node
var gScore = costSoFar[currentNode] + tileCost.Value;
if (costSoFar.TryGetValue(nextNode, out var nextValue) && gScore >= nextValue)
{
continue;
}
cameFrom[nextNode] = currentNode;
costSoFar[nextNode] = gScore;
// pFactor is tie-breaker where the fscore is otherwise equal.
// See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
// There's other ways to do it but future consideration
// The closer the fScore is to the actual distance then the better the pathfinder will be
// (i.e. somewhere between 1 and infinite)
// Can use hierarchical pathfinder or whatever to improve the heuristic but this is fine for now.
var fScore = gScore + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f);
frontier.Add((fScore, nextNode));
}
}
if (!routeFound)
{
return(null);
}
var route = PathfindingHelpers.ReconstructPath(cameFrom, currentNode);
if (route.Count == 1)
{
return(null);
}
#if DEBUG
// Need to get data into an easier format to send to the relevant clients
if (DebugRoute != null && route.Count > 0)
{
var debugCameFrom = new Dictionary <TileRef, TileRef>(cameFrom.Count);
var debugGScores = new Dictionary <TileRef, float>(costSoFar.Count);
foreach (var(node, parent) in cameFrom)
{
debugCameFrom.Add(node.TileRef, parent.TileRef);
}
foreach (var(node, score) in costSoFar)
{
debugGScores.Add(node.TileRef, score);
}
var debugRoute = new SharedAiDebug.AStarRouteDebug(
_pathfindingArgs.Uid,
route,
debugCameFrom,
debugGScores,
DebugTime);
DebugRoute.Invoke(debugRoute);
}
#endif
return(route);
}
public Path NavigateTo(Vector3Int from, Vector3Int to)
{
if (!HasNodeAt(from) || !HasNodeAt(to))
{
return(null);
}
List <PathfindingNode> nodes = new List <PathfindingNode>();
PathfindingNode current = new PathfindingNode(from);
PathfindingNode toNode = new PathfindingNode(to);
for (int x = 0; x < Width; x += 1)
{
for (int y = 0; y < Height; y += 1)
{
if (!_nodes[x, y])
{
continue;
}
PathfindingNode node = new PathfindingNode(x, y);
nodes.Add(node);
node.FScore = node.Heuristic(toNode);
if (node.Equals(from))
{
current = node;
}
if (node.Equals(to))
{
toNode = node;
}
}
}
current.GScore = 0;
List <PathfindingNode> closedSet = new List <PathfindingNode>();
List <PathfindingNode> openSet = new List <PathfindingNode> {
current
};
while (openSet.Count > 0)
{
current = openSet.OrderBy(n => n.FScore).First();
if (current.Equals(to))
{
return(current.ConstructPath());
}
openSet.Remove(current);
closedSet.Add(current);
foreach (PathfindingNode node in nodes.Where(n => n.IsNeighboor(current)))
{
if (closedSet.Contains(node))
{
continue;
}
if (!openSet.Contains(node))
{
openSet.Add(node);
}
int tentativeGScore = current.GScore + 1;
if (tentativeGScore > AStarLimit)
{
return(null);
}
if (tentativeGScore >= node.GScore)
{
continue;
}
node.GScore = tentativeGScore;
node.FScore = node.GScore + node.Heuristic(toNode);
if (current.From != null)
{
node.FScore += current.To(current.From).Equals(node.To(current)) ? 0 : 4;
}
node.From = current;
}
}
return(null);
}
/// <summary>
/// Returns path between two nodes. On the path first node is start and last node is end.
/// TODO: limit range for optimization?
/// TODO: use this for all pathfinding
/// TODO: diagonals?
/// </summary>
/// <param name="all_nodes"></param>
/// <param name="start"></param>
/// <param name="end"></param>
/// <returns></returns>
public static List <PathfindingNode> Path(List <PathfindingNode> all_nodes, PathfindingNode start, PathfindingNode end, bool diagonal_movement)
{
List <PathfindingNode> path = new List <PathfindingNode>();
List <PathfindingNode> Q = new List <PathfindingNode>();
Dictionary <PathfindingNode, float> dist = new Dictionary <PathfindingNode, float>();
Dictionary <PathfindingNode, PathfindingNode> prev = new Dictionary <PathfindingNode, PathfindingNode>();
if (!all_nodes.Contains(end))
{
all_nodes.Add(end);
}
if (!all_nodes.Contains(start))
{
all_nodes.Add(start);
}
for (int i = 0; i < all_nodes.Count; i++)
{
dist.Add(all_nodes[i], float.MaxValue);
prev.Add(all_nodes[i], null);
Q.Add(all_nodes[i]);
}
dist[start] = 0.0f;
///////TODO: Check for impassable tiles and give them dist = float.MaxValue
foreach (KeyValuePair <Coordinates.Direction, PathfindingNode> v in start.Get_Adjanced_Nodes(all_nodes.ToList(), diagonal_movement))
{
dist[v.Value] = Helper.Is_Diagonal(v.Key) ? Mathf.Sqrt(2.0f) * v.Value.Cost : v.Value.Cost;
prev[v.Value] = start;
}
while (Q.Count > 0)
{
int min_dist_index = 0;
float min_dist = -1.0f;
for (int i = 0; i < Q.Count; i++)
{
if (dist[Q[i]] < min_dist || min_dist == -1.0f)
{
min_dist_index = i;
min_dist = dist[Q[i]];
}
}
PathfindingNode u = Q[min_dist_index];
Q.RemoveAt(min_dist_index);
if (u.Equals(end))
{
while (prev[u] != null)
{
path.Insert(0, prev[u]);
u = prev[u];
}
path.Add(end);
break;
}
else
{
foreach (KeyValuePair <Coordinates.Direction, PathfindingNode> v in u.Get_Adjanced_Nodes(all_nodes, diagonal_movement))
{
float alt = Helper.Is_Diagonal(v.Key) ? dist[u] + (Mathf.Sqrt(2.0f) * v.Value.Cost) : dist[u] + v.Value.Cost;
///////TODO: Check for impassable tiles and give them dist = float.MaxValue
if (alt < dist[v.Value])
{
dist[v.Value] = alt;
prev[v.Value] = u;
}
}
}
}
if (path.Count == 1 && path[0] == end)
{
return(new List <PathfindingNode>());
}
return(path);
}
public override bool Equals(object obj)
{
return(to.Equals(obj));
}
