public IDictionary <GridNode, GridNode> Fill(GridNode start, float searchDepth, Motility motility)
{
Dictionary <GridNode, GridNode> parentMap = new Dictionary <GridNode, GridNode>();
SimplePriorityQueue <GridNode> frontier = new SimplePriorityQueue <GridNode>();
frontier.Enqueue(start, 0);
while (frontier.Count > 0)
{
GridNode current = frontier.Dequeue();
foreach (GridNode neighbor in current.Neighbors)
{
IPathfindingEdge edge = current.JoiningEdge(neighbor);
//if (!edge.IsTraversable(edge, motility)) continue;
float newCost = current.PathCost + edge.Weight;
float neighborCost = neighbor.PathCost;
bool containsKey = parentMap.ContainsKey(neighbor);
if (containsKey && (newCost >= neighborCost) || newCost > searchDepth)
{
continue;
}
neighbor.PathCost = newCost;
parentMap[neighbor] = current; // UPSERT dictionary function
float priority = newCost;
frontier.Enqueue(neighbor, priority);
}
}
return(parentMap);
}
public override IList <GridNode> Search(GridNode start, GridNode goal, Motility motility,
Func <GridNode, GridNode, float> heuristic)
{
Dictionary <GridNode, GridNode> parentMap = new Dictionary <GridNode, GridNode>();
SimplePriorityQueue <GridNode> frontier = new SimplePriorityQueue <GridNode>();
frontier.Enqueue(start, start.PathCost);
while (frontier.Count > 0)
{
GridNode current = frontier.Dequeue();
if (current == goal)
{
break;
}
foreach (GridNode neighbor in current.Neighbors)
{
IPathfindingEdge edge = current.JoiningEdge(neighbor);
//if (!IsTraversable(edge, motility)) continue; // TODO: This line should be made irrelevant
float newCost = current.PathCost + edge.Weight;
float neighborCost = neighbor.PathCost;
bool containsKey = parentMap.ContainsKey(neighbor);
if (containsKey && (newCost >= neighborCost))
{
continue;
}
neighbor.PathCost = newCost;
parentMap[neighbor] = current; // UPSERT dictionary function
float priority = newCost + heuristic(goal, neighbor);
frontier.Enqueue(neighbor, priority);
}
}
IList <GridNode> path = ReconstructPath(parentMap, start, goal);
return(path);
}
public void JoiningEdge_NeighborNodes_Edge()
{
Vec2 position = new Vec2(3, 3);
GridNode node = _grid[position];
IPathfindingEdge joiningEdge = node.JoiningEdge(_grid[position + Direction.N]);
IPathfindingEdge edge = new GridEdge(position, Direction.N);
Assert.AreEqual(edge, joiningEdge);
joiningEdge = node.JoiningEdge(_grid[position + Direction.W]);
edge = new GridEdge(position, Direction.W);
Assert.AreEqual(edge, joiningEdge);
joiningEdge = node.JoiningEdge(_grid[position + Direction.E]);
edge = new GridEdge(new Vec2(position.x + 1, position.y), Direction.W);
Assert.AreEqual(edge, joiningEdge);
joiningEdge = node.JoiningEdge(_grid[position + Direction.S]);
edge = new GridEdge(new Vec2(position.x, position.y + 1), Direction.N);
Assert.AreEqual(edge, joiningEdge);
joiningEdge = node.JoiningEdge(_grid[position + Direction.NW]);
edge = new GridVertex(position);
Assert.AreEqual(edge, joiningEdge);
joiningEdge = node.JoiningEdge(_grid[position + Direction.NE]);
edge = new GridVertex(new Vec2(position.x + 1, position.y));
Assert.AreEqual(edge, joiningEdge);
joiningEdge = node.JoiningEdge(_grid[position + Direction.SW]);
edge = new GridVertex(new Vec2(position.x, position.y + 1));
Assert.AreEqual(edge, joiningEdge);
joiningEdge = node.JoiningEdge(_grid[position + Direction.SE]);
edge = new GridVertex(new Vec2(position.x + 1, position.y + 1));
Assert.AreEqual(edge, joiningEdge);
}
public override IList <GridNode> Search(GridNode start, GridNode goal, Motility motility)
{
Dictionary <GridNode, GridNode> parentMap = new Dictionary <GridNode, GridNode>();
Queue <GridNode> frontier = new Queue <GridNode>();
frontier.Enqueue(start);
while (frontier.Count > 0)
{
GridNode current = frontier.Dequeue();
if (current == goal)
{
break;
}
foreach (GridNode neighbor in current.Neighbors)
{
IPathfindingEdge edge = current.JoiningEdge(neighbor);
//if (!edge.IsTraversable(edge, motility)) continue;
if (parentMap.ContainsKey(neighbor))
{
continue;
}
frontier.Enqueue(neighbor);
parentMap.Add(neighbor, current);
}
}
IList <GridNode> path = ReconstructPath(parentMap, start, goal);
return(path);
}
