public override void Update(double elapsed)
{
_speedBoost -= BoostDecrement * elapsed;
_speedBoost = _speedBoost < 1 ? 1 : _speedBoost;
double distance = (Position - _current.Position).Length;
if (distance < TargetDistance)
{
if (_enumerator.MoveNext())
{
_current = _enumerator.Current;
// Recursion stops when we have a target that's far enough
// to move towards.
Update(elapsed);
}
else
{
// Time to explore again!
Initialize();
}
}
else
{
_steering.Location = _current.Position;
Vector2 steeringForce = _steering.Steer(this, elapsed) * 3 * _speedBoost;
Vector2 acceleration = steeringForce / Mass;
Velocity += acceleration * elapsed;
Velocity = Velocity.Truncate(MaxSpeed);
Position += Velocity * elapsed * _speedBoost;
UpdateSpriteDirection();
UpdateSpriteAnimation(elapsed);
}
}
public static Graph CreateGraph(
int x, int y,
int columns, int rows,
float sizePerEdge)
{
Graph graph = new Graph();
GraphNode[,] nodes = new GraphNode[columns, rows];
// Create all GraphNodes.
for (int i = 0; i < columns; i++)
{
for (int j = 0; j < rows; j++)
{
double xPos = x * sizePerEdge + i * sizePerEdge;
double yPos = y * sizePerEdge + j * sizePerEdge;
nodes[i, j] = new GraphNode(new Vector2(xPos, yPos));
}
}
// Create all GraphEdges and add them to the graph.
for (int i = 0; i < columns; i++)
{
for (int j = 0; j < rows; j++)
{
CreateEdges(graph, nodes, i, j, columns, rows);
}
}
return graph;
}
public void ConnectedGraph_GetDepthFirstEnumerable_StartsWithStartNode()
{
Graph graph = new Graph();
GraphNode start = new GraphNode(new Vector2(0, 0));
GraphNode finish = new GraphNode(new Vector2(1, 1));
graph.AddEdge(new GraphEdge(start, finish));
var enumerator = graph.GetDepthFirstEnumerable(start).GetEnumerator();
Assert.IsTrue(enumerator.Current == start,
"Enumerator did not start with the correct node");
}
private static void CreateEdges(Graph graph, GraphNode[,] nodes, int x, int y,
int columns, int rows)
{
for (int i = x - 1; i < x + 2; i++)
{
for (int j = y - 1; j < y + 2; j++)
{
if (i > 0 && i < columns && j > 0 && j < rows &&
!(i == x && j == y))
{
graph.AddEdge(new GraphEdge(nodes[x, y], nodes[i, j]));
}
}
}
}
/// <summary>
/// Recursive call for calculating the shortest path.
/// </summary>
/// <returns>Whether the current node is connected to the destination.</returns>
private bool GetShortestPathRecursive(GraphNode current, GraphNode destination)
{
// Backtrack if we have found a path.
if (current == destination)
{
return true;
}
foreach (GraphEdge edge in current.AdjecentEdges)
{
// If our current node is in the open list...
if (_openList.Any(n => n == edge.Destination))
{
// Update best distance if we have a better distance.
if (current.BestDistance + edge.Cost <
edge.Destination.BestDistance)
{
edge.Destination.BestDistance =
current.BestDistance + edge.Cost;
edge.Destination.EstimatedDistance =
_heuristic.Calculate(edge.Destination);
edge.Destination.Parent = current;
}
}
else if (!_closedList.Any(n => n == edge.Destination))
{
// Update best distance and add node to open list.
edge.Destination.BestDistance =
current.BestDistance + edge.Cost;
edge.Destination.EstimatedDistance =
_heuristic.Calculate(edge.Destination);
edge.Destination.Parent = current;
_openList.Add(edge.Destination);
}
_consideredEdges.Add(edge);
}
_openList.Remove(current);
_closedList.Add(current);
GraphNode next = _openList.FirstOrDefault(n =>
n.Score == _openList.Min(ni => ni.Score)
);
if (next == null)
{
// Can't find a path, go back.
return false;
}
return GetShortestPathRecursive(next, destination);
}
public void GraphWithDisconnectedNodes_GetShortestPath_ThrowsArgumentException()
{
GraphNode source = new GraphNode(new Vector2(0f, 0f));
GraphNode destination = new GraphNode(new Vector2(10f, 10f));
Graph disconnectedGraph = new Graph();
disconnectedGraph.AddEdge(
new GraphEdge(source, new GraphNode(new Vector2(1f, 1f)))
);
disconnectedGraph.AddEdge(
new GraphEdge(new GraphNode(new Vector2(5f, 5f)), destination)
);
AStarAlgorithm astar = new AStarAlgorithm(disconnectedGraph);
// Should throw an argument exception, because it cannot
// calculate the shortest path between disconnected nodes.
astar.GetShortestPath(source, destination);
}
public ShortestPath GetShortestPath(GraphNode start, GraphNode destination)
{
Initialize(destination);
start.BestDistance = 0f;
_consideredEdges = new LinkedList<GraphEdge>();
if (GetShortestPathRecursive(start, destination))
{
var path = new LinkedList<GraphNode>();
GraphNode current = destination;
path.AddFirst(current);
while (current != start)
{
current = current.Parent;
path.AddFirst(current);
}
return new ShortestPath(path.ToList(), _consideredEdges);
}
throw new ArgumentException("Startnode is not connected to destination node.");
}
public void ConnectedGraph_GetShortestPath_ReturnsCorrectPath()
{
// Create a graph with four nodes, connected in a straight line.
Graph graph = new Graph();
const int NodeCount = 4;
GraphNode start = new GraphNode(new Vector2(0f, 0f));
GraphNode midFirst = new GraphNode(new Vector2(1f, 1f));
GraphNode midSecond = new GraphNode(new Vector2(2f, 2f));
GraphNode destination = new GraphNode(new Vector2(3f, 3f));
graph.AddEdge(new GraphEdge(start, midFirst));
graph.AddEdge(new GraphEdge(midFirst, midSecond));
graph.AddEdge(new GraphEdge(midSecond, destination));
AStarAlgorithm astar = new AStarAlgorithm(graph);
IEnumerable<GraphNode> path =
astar.GetShortestPath(start, destination).Path;
Assert.IsTrue(path.Count() == NodeCount,
"Path has an invalid amount of nodes");
}
public void ConnectedGraph_GetDepthFirstEnumerable_TraversesCorrectAmountOfNodes()
{
const int EdgeCount = 4;
const int NodeCount = EdgeCount + 1;
Graph graph = new Graph();
GraphNode start = new GraphNode(new Vector2());
GraphNode currentSource = start;
GraphNode currentDestination;
for (int i = 0; i < EdgeCount; i++)
{
currentDestination = new GraphNode(new Vector2());
graph.AddEdge(new GraphEdge(currentSource, currentDestination));
currentSource = currentDestination;
}
var enumerator = graph.GetDepthFirstEnumerable(start).GetEnumerator();
int actualCount = 1;
while (enumerator.MoveNext())
{
actualCount++;
}
Assert.AreEqual(NodeCount, actualCount,
"DeothFirstEnumerator did not traverse the correct amount of nodes.");
}
private void Initialize()
{
GraphNode nearestNode = _graph.NearestNode(this.Position);
_enumerator = _graph.GetDepthFirstEnumerable(nearestNode).GetEnumerator();
_current = _enumerator.Current;
}
private void Initialize(GraphNode destination)
{
this._openList = new LinkedList<GraphNode>();
this._closedList = new LinkedList<GraphNode>();
this._destination = destination;
this._heuristic = new EuclideanDistanceHeuristic(destination);
}
/// <summary>
/// Returns an enumerator that uses the depth first algorithm to traverse
/// this tree, starting at the specified start node.
/// </summary>
public IEnumerable<GraphNode> GetDepthFirstEnumerable(GraphNode startNode)
{
return new DepthFirstGraph(this, startNode);
}
public int Calculate(GraphNode node)
{
Vector2 distance = node.Position - _destination.Position;
return (int)distance.Length;
}
public EuclideanDistanceHeuristic(GraphNode destination)
{
this._destination = destination;
}
