/// <summary> /// Finds the shortest or otherwise least costly path from the specified source vertex to /// the specified target vertex, using the A* algorithm and the supplied heuristic and cost /// delegates to measure costs between vertices and over vertex edges. /// </summary> /// <param name="source">The source vertex from which the path should start.</param> /// <param name="target">The target vertex that the path should attempt to reach.</param> /// <param name="costHeuristic">Delegate for estimating the cost of the path from the specified source vertex to the specified target vertex.</param> /// <param name="cost">Delegate for determining the actual cost of the path along the specified vertex edge, from its near vertex to its far vertex.</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 vertex 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 IVertexEdgePath FindPath(Topology.Vertex source, Topology.Vertex target, VertexCostHeuristicDelegate costHeuristic, VertexCostDelegate cost, IVertexEdgePath path = null) { if (!source) { throw new ArgumentException("The source vertex must be a valid vertex.", "source"); } if (!target) { throw new ArgumentException("The target vertex must be a valid vertex.", "target"); } if (!source.topology != target.topology) { throw new ArgumentException("The target vertex must belong to the same topology as the source vertex.", "target"); } var concretePath = path as VertexEdgePath; 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 VertexEdgePath(); } 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.vertices.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 vertex = new Topology.Vertex(topology, node._elementIndex); foreach (var edge in vertex.edges) { if (_closedSet.ContainsKey(edge.vertex.index)) { continue; } var g = node._g + cost(edge, node._length); if (!float.IsPositiveInfinity(g)) { Node neighborNode; if (!_openSet.TryGetValue(edge.vertex.index, out neighborNode)) { var h = costHeuristic(edge.vertex, target, node._length); neighborNode = new Node(g + h, g, h, edge.vertex.index, edge.index, node._elementIndex, node._length + 1); _queue.Push(neighborNode); _openSet.Add(edge.vertex.index, neighborNode); } else if (g < neighborNode._g) { var h = costHeuristic(edge.vertex, target, node._length); neighborNode = new Node(g + h, g, h, edge.vertex.index, edge.index, node._elementIndex, node._length + 1); _openSet[edge.vertex.index] = neighborNode; _queue.Reprioritize(neighborNode); } } } } return(concretePath.Rebuild(source, target)); }
public VertexPathAdapter(VertexEdgePath vertexEdgePath) { _vertexEdgePath = vertexEdgePath; }