/// <summary>
/// Finds the shortest or otherwise least costly path from the specified source face to
/// the specified target face, using the A* algorithm and the supplied heuristic and cost
/// delegates to measure costs between faces and over face edges.
/// </summary>
/// <param name="source">The source face from which the path should start.</param>
/// <param name="target">The target face that the path should attempt to reach.</param>
/// <param name="costHeuristic">Delegate for estimating the cost of the path from the specified source face to the specified target face.</param>
/// <param name="cost">Delegate for determining the actual cost of the path along the specified face edge, from its near vertex to its far face.</param>
/// <param name="path">An optional existing path created by an earlier call to one of the <seealso cref="O:MakeIt.Tile.PathFinder.FindPath"/> functions, which will be overwritten with the new path data.</param>
/// <returns>A face edge path instance describing the path found from source to target, or an incomplete object if no path was found.</returns>
/// <remarks><para>The optional <paramref name="path"/> parameter is useful for reducing allocation activity
/// and pressure on the garbage collector. Reusing an existing path object will not require an additional
/// allocation to store the path as long as the new path fits inside the capacity already available in the
/// existing path.</para></remarks>
public IFaceEdgePath FindPath(Topology.Face source, Topology.Face target, FaceCostHeuristicDelegate costHeuristic, FaceCostDelegate cost, IFaceEdgePath path = null)
{
if (!source)
{
throw new ArgumentException("The source face must be a valid face.", "source");
}
if (!target)
{
throw new ArgumentException("The target face must be a valid face.", "target");
}
if (source.topology != target.topology)
{
throw new ArgumentException("The target face must belong to the same topology as the source face.", "target");
}
var concretePath = path as FaceEdgePath;
if (concretePath == null)
{
if (path != null)
{
throw new ArgumentException("The provided pre-allocated path was not an instance of the necessary underlying type recognized by this path finder.", "path");
}
concretePath = new FaceEdgePath();
}
var topology = source.topology;
if (source == target)
{
return(concretePath.Rebuild(source, target));
}
if (_queue == null)
{
_queue = new DelegateOrderedPriorityQueue <Node>(Node.AreOrdered, Mathf.CeilToInt(Mathf.Sqrt(source.topology.faces.Count)));
_openSet = new Dictionary <int, Node>();
_closedSet = new Dictionary <int, Node>();
}
else
{
_queue.Clear();
_openSet.Clear();
_closedSet.Clear();
}
_queue.Push(new Node(0f, 0f, costHeuristic(source, target, 0), source.index, -1, -1, 0));
_openSet.Add(source.index, _queue.front);
while (_queue.Count > 0)
{
var node = _queue.front;
_queue.Pop();
if (node._elementIndex == target.index)
{
return(concretePath.Rebuild(source, target, node, _closedSet));
}
_closedSet.Add(node._elementIndex, node);
var face = new Topology.Face(topology, node._elementIndex);
foreach (var edge in face.edges)
{
if (_closedSet.ContainsKey(edge.face.index))
{
continue;
}
var g = node._g + cost(edge, node._length);
if (!float.IsPositiveInfinity(g))
{
Node neighborNode;
if (!_openSet.TryGetValue(edge.face.index, out neighborNode))
{
var h = costHeuristic(edge.face, target, node._length);
neighborNode = new Node(g + h, g, h, edge.face.index, edge.index, node._elementIndex, node._length + 1);
_queue.Push(neighborNode);
_openSet.Add(edge.face.index, neighborNode);
}
else if (g < neighborNode._g)
{
var h = costHeuristic(edge.face, target, node._length);
neighborNode = new Node(g + h, g, h, edge.face.index, edge.index, node._elementIndex, node._length + 1);
_openSet[edge.face.index] = neighborNode;
_queue.Reprioritize(neighborNode);
}
}
}
}
return(concretePath.Rebuild(source, target));
}
public FacePathAdapter(FaceEdgePath faceEdgePath)
{
_faceEdgePath = faceEdgePath;
}
