/// <summary>
/// Neighbour handling of our AStar algorithm.
/// </summary>
private void HandleNeighbours(AStarNode current, List<MeshNode> neighbours, MinHeap<AStarNode> workingQueue, PointSet<float> workingCoords, PointSet<float> exploredCoords, Vector2 end)
{
foreach (MeshNode neighbour in neighbours)
{
// neighbour will be added if neither in workingQueue(workingCoords) nor exploredCoords
if (!exploredCoords.Contains(neighbour.mVector.X, neighbour.mVector.Y) && !workingCoords.Contains(neighbour.mVector.X, neighbour.mVector.Y))
{
double neighbourCostsToEnd = HelperMethods.EuklidDistance(neighbour.mVector, end);
double neighbourLatestCosts =
current.mLatestCosts + HelperMethods.EuklidDistance(current.mNode.mVector, neighbour.mVector);
AStarNode neighbour2 = new AStarNode(neighbourLatestCosts, neighbourCostsToEnd, neighbour, current);
// add neighbour with its heuristic value to workingQueue:
workingQueue.Add(HeuristicValue(neighbour2), neighbour2);
workingCoords.Add(neighbour.mVector.X, neighbour.mVector.Y);
}
}
}
/// <summary>
/// Returns the heuristic value for a node. In our case
/// it is just its latest costs plus the air distance to the target.
/// </summary>
/// <param name="current">node of which the value should be returned</param>
/// <returns>heuristic value of 'current'</returns>
private double HeuristicValue(AStarNode current)
{
return current.mLatestCosts + current.mCostsToEnd;
}
/// <summary>
/// Prepares the result for CalculatePath2().
/// </summary>
private Path HandleEnding(AStarNode current, Vector2 end, bool startPassable, bool endPassable, Polygon endPolygon)
{
// we're done!
AStarNode last;
if (endPassable && !current.mNode.mVector.Equals(end))
// We really want to get to the end point!
last = new AStarNode(0, 0, new MeshNode(end), current);
else
// We don't need to reach the end point
last = current;
// path cleanup needed?
if (GlobalValues.GetInstance().mCleanUpPath)
return PathCleaner.GetCleanedUpVectorPath(last, startPassable, endPolygon);
/*else*/
return GetPath(last, startPassable);
}
/// <summary>
/// Returns the non-cleaned-up path.
/// </summary>
/// <param name="target">the last end AStarNode</param>
/// <param name="startPassable">if start is passable</param>
private Path GetPath(AStarNode target, bool startPassable)
{
Stack<Vector2> stack = new Stack<Vector2>();
while (target != null)
{
stack.Push(target.mNode.mVector);
target = target.mPrecedent;
}
// If we don't need the start point, we just pop the most recent point:
if (!startPassable) stack.Pop();
return new Path(stack);
}
/// <summary>
/// Checks the end condition of our AStar algorithm.
/// </summary>
private bool EndConditionMet(AStarNode current, bool endPassable, Polygon endPolygon, List<MeshNode> endNodes)
{
if (endPassable)
{ // use endPolygon to determine termination
// we're done if the current node is part of the endPolygon
return (current.mNode.mPolyOwners.Contains(endPolygon));
}
/*else*/
{ // use endNodes to determine termination
// we're done if the current node is part of endNodes:
return (endNodes.Contains(current.mNode));
}
}
/// <summary>
/// This does the actual work for CalculatePath(). CalculatePath() did just
/// determine all the parameters for it.
/// </summary>
private Path CalculatePath2(Vector2 end, MeshNode startMeshNode, List<MeshNode> firstNeighbours, Polygon endPolygon, List<MeshNode> endNodes, bool startPassable, bool endPassable)
{
Debug.Assert(mMesh != null, "There is no navigation mesh! Load a map and then call PathFinder.GetInstance().LoadMeshFromObstructions().");
// uses A* algorithm
// !! remember to distinguish between latest costs (costs up to that point)
// !! and heuristic costs (estimated path costs over that point to target)
// I use AStarNode instead of MeshNode / Vector2 because I have to save
// additional information, like predecessor and latest costs.
// WorkingQueue should be in ascending order
// of heuristic value. I use my own comparer to allow multiple
// equal keys (since we may have nodes with the same heuristic value).
MinHeap<AStarNode> workingQueue = new MinHeap<AStarNode>();
// workingQueue doesn't support a quick check if some coords are
// already contained, so we handle that with a corresp. structure:
PointSet<float> workingCoords = new PointSet<float>();
PointSet<float> exploredCoords = new PointSet<float>();
// TODO: Evaluate First Loop iteration separately
double startCostsToEnd = HelperMethods.EuklidDistance(startMeshNode.mVector, end);
AStarNode current = new AStarNode(0, startCostsToEnd, startMeshNode, null);
if (EndConditionMet(current, endPassable, endPolygon, endNodes))
return HandleEnding(current, end, startPassable, endPassable, endPolygon);
exploredCoords.Add(startMeshNode.mVector.X, startMeshNode.mVector.Y);
HandleNeighbours(current, firstNeighbours, workingQueue, workingCoords, exploredCoords, end);
// TODO: First iteration evaluation finished
while (workingQueue.Count > 0)
{
//Debug.WriteLine("No. of nodes in the workingQueue: "+workingQueue.Count);
//OutputHeuristics(workingQueue);
current = GetNextNodeFromWorkingQueue(workingQueue, workingCoords);
//Debug.WriteLine("Now examining "+current.mNode.mVector.X+","+current.mNode.mVector.Y);
if (EndConditionMet(current, endPassable, endPolygon, endNodes))
return HandleEnding(current, end, startPassable, endPassable, endPolygon);
// The shortest path to current has been found,
// so we won't have to touch it ever again!
exploredCoords.Add(current.mNode.mVector.X, current.mNode.mVector.Y);
//Debug.WriteLine("Explored " + current.mNode.mVector.X + "," + current.mNode.mVector.Y);
// Add all unexplored unblocked neighbours to workingQueue:
List<MeshNode> neighbours = GetNeighbours2(current.mNode);
HandleNeighbours(current, neighbours, workingQueue, workingCoords, exploredCoords, end);
}
// Working queue got empty, but we still didn't reach our target
// -> No path found
Debug.WriteLine("WARNING - Pathfinder: No path found!");
return null;
}
/// <summary>
/// Takes the result from PathFinder.CalculatePath() and returns a cleaned
/// up path from it.
/// </summary>
public static Path GetCleanedUpVectorPath(AStarNode target, bool startPassable, Polygon endPolygon)
{
// First get the list of meshNodes:
List<MeshNode> meshNodes = new List<MeshNode>();
while (target != null)
{
meshNodes.Add(target.mNode);
target = target.mPrecedent;
}
// If we don't need to reach the start point, we just remove the most recent point:
if (!startPassable) meshNodes.RemoveAt(meshNodes.Count - 1);
meshNodes.Reverse();
// We're ready to kick ass!
if (meshNodes.Count > 2)
{ // We can only clean up with 3 or more nodes
int uniteFromIndex = 0;
while (uniteFromIndex < meshNodes.Count - 2)
{
Debug.Assert(uniteFromIndex < meshNodes.Count - 2);
int i = uniteFromIndex + 2;
Debug.Assert(i < meshNodes.Count);
Stack<Edge> firstCandidateEdges = new Stack<Edge>(meshNodes[uniteFromIndex].GetPolyOwnersEdges());
Debug.Assert(firstCandidateEdges.Count > 0,
"GetCleanedUpVectorPath(): Could not find first candidate edges!");
Debug.Assert(uniteFromIndex < i - 1);
Debug.Assert(uniteFromIndex < meshNodes.Count);
Debug.Assert(i < meshNodes.Count);
while (i < meshNodes.Count && IsCleanUpPossible(GetCleanUpEndPolygons(i, meshNodes, endPolygon),
firstCandidateEdges,
meshNodes[uniteFromIndex].mVector,
meshNodes[i].mVector))
{
i++;
}
// Whatever case has broken the loop, we try to
// clean up from uniteFromIndex to i-1 if feasible:
i--;
if (uniteFromIndex < i - 1)
{
int removedNodeCount = i - uniteFromIndex - 1;
Debug.Assert(removedNodeCount > 0);
CleanUp(uniteFromIndex, i, meshNodes);
}
uniteFromIndex++;
}
// uniteFromIndex is too high to clean up anymore
}
// We're done:
Stack<Vector2> stack = new Stack<Vector2>();
// fill path backwards (it's a stack after all):
for (int j = meshNodes.Count - 1; j >= 0; j--)
{
stack.Push(meshNodes[j].mVector);
}
return new Path(stack);
}