public bool MoveNext() { while (m_parent.m_bfsQueue.Count() != 0) { m_currentVertex = m_parent.m_bfsQueue.Dequeue(); V otherVertex = null; for (int i = 0; i < m_currentVertex.GetNeighborCount(); ++i) { if (m_edgeTraversable == null) { otherVertex = (V)m_currentVertex.GetNeighbor(i); if (otherVertex == null) { continue; } } else { IMyPathEdge <V> otherEdge = m_currentVertex.GetEdge(i); if (!m_edgeTraversable(otherEdge)) { continue; } otherVertex = otherEdge.GetOtherVertex(m_currentVertex); if (otherVertex == null) { continue; } } if (otherVertex.PathfindingData.Timestamp != m_parent.m_timestamp && (m_vertexTraversable == null || m_vertexTraversable(otherVertex))) { m_parent.m_bfsQueue.Enqueue(otherVertex); otherVertex.PathfindingData.Timestamp = m_parent.m_timestamp; } } if (m_vertexFilter == null || m_vertexFilter(m_currentVertex)) { return(true); } } return(false); }
// Note: Termination criterion tells the pathfinding system, how much we want to terminate the pathfinding in the given vertex. // // Values can range from 0.0 to positive infinity. Infinity means that the vertex will never be returned as a result. Zero, on the // other hand, is equivalent to classical pathfinding, which means that the pathfinding will terminate when a path has been found // to a vertex with a criterion value of zero and that path's length could not be improved by looking further. // // Non-zero values on vertices tell the pathfinding that these vertices are acceptable as final vertices, but only if no better // vertex is found (i.e. one that would have lower length + heuristic value). // // The termination criterion is not used as weighting for the pathfinding priority queue though, which means that you can separate // termination in a vertex from the searching heuristic public MyPath <V> FindPath(V start, Func <V, float> heuristic, Func <V, float> terminationCriterion, Predicate <V> vertexTraversable = null, bool returnClosest = true) { Debug.Assert(heuristic != null); Debug.Assert(terminationCriterion != null); CalculateNextTimestamp(); MyPathfindingData startData = start.PathfindingData; Visit(startData); startData.Predecessor = null; startData.PathLength = 0.0f; IMyPathVertex <V> retVal = null; float lowestAcceptableWeight = float.PositiveInfinity; IMyPathVertex <V> closest = null; float closestWeight = float.PositiveInfinity; float terminationValue = terminationCriterion(start); if (terminationValue != float.PositiveInfinity) { retVal = start; lowestAcceptableWeight = heuristic(start) + terminationValue; } m_openVertices.Insert(start.PathfindingData, heuristic(start)); while (m_openVertices.Count > 0) { MyPathfindingData currentData = m_openVertices.RemoveMin(); V current = currentData.Parent as V; float currentPathLength = currentData.PathLength; if (retVal != null && currentPathLength + heuristic(current) >= lowestAcceptableWeight) { break; } for (int i = 0; i < current.GetNeighborCount(); ++i) { IMyPathEdge <V> edge = current.GetEdge(i); if (edge == null) { continue; } V neighbor = edge.GetOtherVertex(current); if (neighbor == null || (vertexTraversable != null && !vertexTraversable(neighbor))) { continue; } float newPathLength = currentData.PathLength + edge.GetWeight(); MyPathfindingData neighborData = neighbor.PathfindingData; float neighborWeight = newPathLength + heuristic(neighbor); if (neighborWeight < closestWeight) { closest = neighbor; closestWeight = neighborWeight; } terminationValue = terminationCriterion(neighbor); if (neighborWeight + terminationValue < lowestAcceptableWeight) { retVal = neighbor; lowestAcceptableWeight = neighborWeight + terminationValue; } if (Visited(neighborData)) { if (newPathLength < neighborData.PathLength) { neighborData.PathLength = newPathLength; neighborData.Predecessor = currentData; m_openVertices.ModifyUp(neighborData, neighborWeight); } } else { Visit(neighborData); neighborData.PathLength = newPathLength; neighborData.Predecessor = currentData; m_openVertices.Insert(neighborData, neighborWeight); } } } m_openVertices.Clear(); if (retVal == null) { if (returnClosest == false || closest == null) { return(null); } else { return(ReturnPath(closest.PathfindingData, null, 0)); } } else { return(ReturnPath(retVal.PathfindingData, null, 0)); } }
public MyPath <V> FindPath(V start, V end, Predicate <V> vertexTraversable = null, Predicate <IMyPathEdge <V> > edgeTraversable = null) { // CH: TODO: Make multiple private copies of this method and call the right one // according to what were the arguments to the public interface method CalculateNextTimestamp(); MyPathfindingData startData = start.PathfindingData; Visit(startData); startData.Predecessor = null; startData.PathLength = 0.0f; IMyPathVertex <V> retVal = null; float shortestPathLength = float.PositiveInfinity; m_openVertices.Insert(start.PathfindingData, start.EstimateDistanceTo(end)); while (m_openVertices.Count > 0) { MyPathfindingData currentData = m_openVertices.RemoveMin(); V current = currentData.Parent as V; float currentPathLength = currentData.PathLength; if (retVal != null && currentPathLength >= shortestPathLength) { break; } for (int i = 0; i < current.GetNeighborCount(); ++i) { /*IMyPathVertex<V> neighbor = current.GetNeighbor(i); * if (neighbor == null) continue;*/ IMyPathEdge <V> edge = current.GetEdge(i); if (edge == null || (edgeTraversable != null && !edgeTraversable(edge))) { continue; } V neighbor = edge.GetOtherVertex(current); if (neighbor == null || (vertexTraversable != null && !vertexTraversable(neighbor))) { continue; } float newPathLength = currentData.PathLength + edge.GetWeight(); MyPathfindingData neighborData = neighbor.PathfindingData; if (neighbor == end && newPathLength < shortestPathLength) { retVal = neighbor; shortestPathLength = newPathLength; } if (Visited(neighborData)) { if (newPathLength < neighborData.PathLength) { neighborData.PathLength = newPathLength; neighborData.Predecessor = currentData; m_openVertices.ModifyUp(neighborData, newPathLength + neighbor.EstimateDistanceTo(end)); } } else { Visit(neighborData); neighborData.PathLength = newPathLength; neighborData.Predecessor = currentData; m_openVertices.Insert(neighborData, newPathLength + neighbor.EstimateDistanceTo(end)); } } } m_openVertices.Clear(); if (retVal == null) { return(null); } else { return(ReturnPath(retVal.PathfindingData, null, 0)); } }