private static float HlCriterion(MyNavigationPrimitive primitive)
{
MyHighLevelPrimitive item = primitive as MyHighLevelPrimitive;
if ((item == null) || m_pathfindingStatic.m_ignoredPrimitives.Contains(item))
{
return(float.PositiveInfinity);
}
float num = m_pathfindingStatic.m_destination.PointAdmissibility(primitive.WorldPosition, 8.7f);
if (num < float.PositiveInfinity)
{
return(num * 4f);
}
IMyHighLevelComponent component = item.GetComponent();
return((component != null) ? (component.FullyExplored ? float.PositiveInfinity : (((float)Vector3D.RectangularDistance(primitive.WorldPosition, m_pathfindingStatic.m_destinationCenter)) * 8f)) : float.PositiveInfinity);
}
private void ExpandPath(Vector3D currentPosition)
{
if (m_pathNodePosition >= m_pathNodes.Count - 1)
{
ProfilerShort.Begin("GenerateHighLevelPath");
GenerateHighLevelPath();
ProfilerShort.End();
}
if (m_pathNodePosition >= m_pathNodes.Count)
{
return;
}
MyPath <MyNavigationPrimitive> path = null;
bool isLastPath = false;
m_expandedPath.Clear();
if (m_pathNodePosition + 1 < m_pathNodes.Count)
{
if (m_pathNodes[m_pathNodePosition].IsExpanded)
{
if (m_pathNodes[m_pathNodePosition + 1].IsExpanded)
{
IMyHighLevelComponent component = m_pathNodes[m_pathNodePosition].GetComponent();
IMyHighLevelComponent otherComponent = m_pathNodes[m_pathNodePosition + 1].GetComponent();
// CH: TODO: Preallocate the functions to avoid using lambdas.
ProfilerShort.Begin("FindPath to next compo");
path = m_pathfinding.FindPath(m_currentPrimitive, m_goal.PathfindingHeuristic, (prim) => otherComponent.Contains(prim) ? 0.0f : float.PositiveInfinity, (prim) => component.Contains(prim) || otherComponent.Contains(prim));
ProfilerShort.End();
}
else
{
Debug.Assert(!MyFakes.SHOW_PATH_EXPANSION_ASSERTS, "First hierarchy path node is expanded, but the second one is not! First two nodes should always be expanded so that pathfinding can be done.");
}
}
else
{
Debug.Assert(!MyFakes.SHOW_PATH_EXPANSION_ASSERTS, "Nodes of smart path are not expanded!");
}
}
else
{
if (m_pathNodes[m_pathNodePosition].IsExpanded)
{
// Try to find a path to a goal primitive inside the last high level component.
// If the last primitive of the found path is not in the last high level component, add that component to the goal's ignore list
IMyHighLevelComponent component = m_pathNodes[m_pathNodePosition].GetComponent();
ProfilerShort.Begin("FindPath to goal in the last component");
path = m_pathfinding.FindPath(m_currentPrimitive, m_goal.PathfindingHeuristic, (prim) => component.Contains(prim) ? m_goal.TerminationCriterion(prim) : 30.0f, (prim) => component.Contains(prim));
ProfilerShort.End();
if (path != null)
{
// We reached goal
if (path.Count != 0 && component.Contains(path[path.Count - 1].Vertex as MyNavigationPrimitive))
{
isLastPath = true;
}
// We reached other component (goal could not be reached in this component)
else
{
m_goal.IgnoreHighLevel(m_pathNodes[m_pathNodePosition]);
}
}
}
else
{
Debug.Assert(!MyFakes.SHOW_PATH_EXPANSION_ASSERTS, "Nodes of smart path are not expanded!");
}
}
if (path == null || path.Count == 0)
{
return;
}
Vector3D end = default(Vector3D);
var lastPrimitive = path[path.Count - 1].Vertex as MyNavigationPrimitive;
if (isLastPath)
{
Vector3 endPoint = m_goal.Destination.GetBestPoint(lastPrimitive.WorldPosition);
Vector3 localEnd = lastPrimitive.Group.GlobalToLocal(endPoint);
localEnd = lastPrimitive.ProjectLocalPoint(localEnd);
end = lastPrimitive.Group.LocalToGlobal(localEnd);
}
else
{
end = lastPrimitive.WorldPosition;
}
RefineFoundPath(ref currentPosition, ref end, path);
// If the path is too short, don't use it to prevent jerking in stuck situations
if (m_pathNodes.Count <= 1 && isLastPath && m_expandedPath.Count > 0 && path.Count <= 2 && !m_goal.ShouldReinitPath())
{
Vector4D end4D = m_expandedPath[m_expandedPath.Count - 1];
// Here, 256 is just a small enough value to catch most false-positives
if (Vector3D.DistanceSquared(currentPosition, end) < end4D.W * end4D.W / 256)
{
m_expandedPath.Clear();
}
}
}
private void ExpandPath(Vector3D currentPosition)
{
if (this.m_pathNodePosition >= (this.m_pathNodes.Count - 1))
{
this.GenerateHighLevelPath();
}
if (this.m_pathNodePosition < this.m_pathNodes.Count)
{
MyPath <MyNavigationPrimitive> path = null;
bool flag = false;
this.m_expandedPath.Clear();
if ((this.m_pathNodePosition + 1) < this.m_pathNodes.Count)
{
if (this.m_pathNodes[this.m_pathNodePosition].IsExpanded && this.m_pathNodes[this.m_pathNodePosition + 1].IsExpanded)
{
IMyHighLevelComponent component = this.m_pathNodes[this.m_pathNodePosition].GetComponent();
IMyHighLevelComponent otherComponent = this.m_pathNodes[this.m_pathNodePosition + 1].GetComponent();
path = this.m_pathfinding.FindPath(this.m_currentPrimitive, this.m_goal.PathfindingHeuristic, prim => otherComponent.Contains(prim) ? 0f : float.PositiveInfinity, prim => component.Contains(prim) || otherComponent.Contains(prim), true);
}
}
else if (this.m_pathNodes[this.m_pathNodePosition].IsExpanded)
{
IMyHighLevelComponent component1 = this.m_pathNodes[this.m_pathNodePosition].GetComponent();
path = this.m_pathfinding.FindPath(this.m_currentPrimitive, this.m_goal.PathfindingHeuristic, prim => component1.Contains(prim) ? this.m_goal.TerminationCriterion(prim) : 30f, prim => component1.Contains(prim), true);
if (path != null)
{
if ((path.Count == 0) || !component1.Contains(path[path.Count - 1].Vertex as MyNavigationPrimitive))
{
this.m_goal.IgnoreHighLevel(this.m_pathNodes[this.m_pathNodePosition]);
}
else
{
flag = true;
}
}
}
if ((path != null) && (path.Count != 0))
{
Vector3D end = new Vector3D();
MyNavigationPrimitive vertex = path[path.Count - 1].Vertex as MyNavigationPrimitive;
if (!flag)
{
end = vertex.WorldPosition;
}
else
{
Vector3 bestPoint = (Vector3)this.m_goal.Destination.GetBestPoint(vertex.WorldPosition);
Vector3 localPos = vertex.ProjectLocalPoint(vertex.Group.GlobalToLocal(bestPoint));
end = vertex.Group.LocalToGlobal(localPos);
}
this.RefineFoundPath(ref currentPosition, ref end, path);
if ((((this.m_pathNodes.Count <= 1) & flag) && ((this.m_expandedPath.Count > 0) && (path.Count <= 2))) && !this.m_goal.ShouldReinitPath())
{
Vector4D vectord2 = this.m_expandedPath[this.m_expandedPath.Count - 1];
if (Vector3D.DistanceSquared(currentPosition, end) < ((vectord2.W * vectord2.W) / 256.0))
{
this.m_expandedPath.Clear();
}
}
}
}
}
