internal void Add(IMyPathVertex <V> vertex, IMyPathVertex <V> nextVertex)
{
PathNode node = new PathNode();
node.Vertex = vertex;
if (nextVertex == null)
{
m_vertices.Add(node);
return;
}
int count = vertex.GetNeighborCount();
for (int i = 0; i < count; ++i)
{
var neigh = vertex.GetNeighbor(i);
if (neigh == nextVertex)
{
node.nextVertex = i;
m_vertices.Add(node);
return;
}
}
Debug.Assert(false, "Could not get from vertex to its neighbor");
}
IMyPathVertex <MyNavigationPrimitive> IMyPathVertex <MyNavigationPrimitive> .GetNeighbor(int index)
{
ProfilerShort.Begin("GetNeighbor");
int ownNeighbors = GetOwnNeighborCount();
IMyPathVertex <MyNavigationPrimitive> neighbor = null;
if (index < ownNeighbors)
{
neighbor = GetOwnNeighbor(index);
}
else
{
neighbor = Group.GetExternalNeighbor(this, index - ownNeighbors);
}
ProfilerShort.End();
return(neighbor);
}
float IMyPathVertex <MyNavigationPrimitive> .EstimateDistanceTo(IMyPathVertex <MyNavigationPrimitive> other)
{
ProfilerShort.Begin("Pathfinding heuristic");
var otherPrimitive = other as MyNavigationPrimitive;
float dist;
if (Group == otherPrimitive.Group)
{
dist = Vector3.Distance(this.Position, otherPrimitive.Position);
}
else
{
dist = (float)Vector3D.Distance(this.WorldPosition, otherPrimitive.WorldPosition);
}
ProfilerShort.End();
return(dist);
}
float IMyPathVertex <IMyConveyorEndpoint> .EstimateDistanceTo(IMyPathVertex <IMyConveyorEndpoint> other)
{
return(Vector3.RectangularDistance((other as IMyConveyorEndpoint).CubeBlock.WorldMatrix.Translation, this.CubeBlock.WorldMatrix.Translation));
}
// 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));
}
}
float IMyPathVertex <MyNavigationPrimitive> .EstimateDistanceTo(IMyPathVertex <MyNavigationPrimitive> other)
{
MyNavigationPrimitive primitive = other as MyNavigationPrimitive;
return(!ReferenceEquals(this.Group, primitive.Group) ? ((float)Vector3D.Distance(this.WorldPosition, primitive.WorldPosition)) : Vector3.Distance(this.Position, primitive.Position));
}
