public override MyHighLevelPrimitive GetHighLevelPrimitive(MyNavigationPrimitive myNavigationTriangle)
{
return(m_higherLevelHelper.GetHighLevelNavigationPrimitive(myNavigationTriangle as MyNavigationTriangle));
}
private float Criterion(MyNavigationPrimitive primitive)
{
return(m_destination.PointAdmissibility(primitive.WorldPosition, 2.0f)); // Triangles of large cube blocks will fit into 2m diam. circumsphere
}
private static float HlHeuristic(MyNavigationPrimitive primitive)
{
return((float)Vector3D.RectangularDistance(primitive.WorldPosition, m_pathfindingStatic.m_destinationCenter) * 2.0f);
}
public MyPath <MyNavigationPrimitive> FindPath(MyPathfinding pathfinding, MyNavigationPrimitive startPrimitive)
{
throw new NotImplementedException();
}
private float Heuristic(MyNavigationPrimitive primitive)
{
return((float)Vector3D.Distance(primitive.WorldPosition, m_destinationCenter));
}
private void RefineFoundPath(ref Vector3D begin, ref Vector3D end, MyPath <MyNavigationPrimitive> path)
{
Debug.Assert(MyFakes.ENABLE_PATHFINDING, "Pathfinding is not enabled!");
if (!MyFakes.ENABLE_PATHFINDING)
{
return;
}
if (path == null)
{
Debug.Assert(false, "Path to refine was null!");
return;
}
m_currentPrimitive = path[path.Count - 1].Vertex as MyNavigationPrimitive;
if (m_hlBegin != null && !m_pathNodes.Contains(m_hlBegin))
{
m_hlBegin.Parent.StopObservingPrimitive(m_hlBegin, this);
}
m_hlBegin = m_currentPrimitive.GetHighLevelPrimitive();
if (m_hlBegin != null && !m_pathNodes.Contains(m_hlBegin))
{
m_hlBegin.Parent.ObservePrimitive(m_hlBegin, this);
}
ProfilerShort.Begin("Path refining and post-processing");
IMyNavigationGroup prevGroup = null;
int groupStart = 0;
int groupEnd = 0;
Vector3 prevBegin = default(Vector3);
Vector3 prevEnd = default(Vector3);
for (int i = 0; i < path.Count; ++i)
{
var primitive = path[i].Vertex as MyNavigationPrimitive;
var group = primitive.Group;
if (prevGroup == null)
{
prevGroup = group;
prevBegin = prevGroup.GlobalToLocal(begin);
}
bool lastPrimitive = i == path.Count - 1;
if (group != prevGroup)
{
groupEnd = i - 1;
prevEnd = prevGroup.GlobalToLocal(primitive.WorldPosition);
}
else if (lastPrimitive)
{
groupEnd = i;
prevEnd = prevGroup.GlobalToLocal(end);
}
else
{
continue;
}
int refinedBegin = m_expandedPath.Count;
prevGroup.RefinePath(path, m_expandedPath, ref prevBegin, ref prevEnd, groupStart, groupEnd);
int refinedEnd = m_expandedPath.Count;
for (int j = refinedBegin; j < refinedEnd; ++j)
{
Vector3D position = new Vector3D(m_expandedPath[j]);
position = prevGroup.LocalToGlobal(position);
m_expandedPath[j] = new Vector4D(position, m_expandedPath[j].W);
}
if (lastPrimitive && group != prevGroup)
{
m_expandedPath.Add(new Vector4D(primitive.WorldPosition, m_expandedPath[refinedEnd - 1].W));
}
prevGroup = group;
groupStart = i;
if (m_expandedPath.Count != 0)
{
prevBegin = group.GlobalToLocal(new Vector3D(m_expandedPath[m_expandedPath.Count - 1]));
}
}
m_pathNodePosition++;
//m_expandedPath.RemoveAt(0);
m_expandedPathPosition = 0;
ProfilerShort.End();
}
private static bool ProcessTriangleForHierarchyStatic(MyNavigationPrimitive primitive)
{
MyNavigationTriangle triangle = primitive as MyNavigationTriangle;
return(m_currentHelper.ProcessTriangleForHierarchy(triangle));
}
bool IMyHighLevelComponent.Contains(MyNavigationPrimitive primitive)
{
MyNavigationTriangle triangle = primitive as MyNavigationTriangle;
return((triangle != null) ? (triangle.ComponentIndex == this.m_componentIndex) : false);
}
private void RefineFoundPath(ref Vector3D begin, ref Vector3D end, MyPath <MyNavigationPrimitive> path)
{
if (MyPerGameSettings.EnablePathfinding && (path != null))
{
this.m_currentPrimitive = path[path.Count - 1].Vertex as MyNavigationPrimitive;
if ((this.m_hlBegin != null) && !this.m_pathNodes.Contains(this.m_hlBegin))
{
this.m_hlBegin.Parent.StopObservingPrimitive(this.m_hlBegin, this);
}
this.m_hlBegin = this.m_currentPrimitive.GetHighLevelPrimitive();
if ((this.m_hlBegin != null) && !this.m_pathNodes.Contains(this.m_hlBegin))
{
this.m_hlBegin.Parent.ObservePrimitive(this.m_hlBegin, this);
}
IMyNavigationGroup objB = null;
int num = 0;
int num2 = 0;
Vector3 startPoint = new Vector3();
Vector3 endPoint = new Vector3();
int num3 = 0;
while (true)
{
while (true)
{
if (num3 >= path.Count)
{
this.m_pathNodePosition++;
this.m_expandedPathPosition = 0;
return;
}
MyNavigationPrimitive vertex = path[num3].Vertex as MyNavigationPrimitive;
IMyNavigationGroup group = vertex.Group;
if (objB == null)
{
objB = group;
startPoint = objB.GlobalToLocal(begin);
}
bool flag = num3 == (path.Count - 1);
if (!ReferenceEquals(group, objB))
{
num2 = num3 - 1;
endPoint = objB.GlobalToLocal(vertex.WorldPosition);
}
else
{
if (!flag)
{
break;
}
num2 = num3;
endPoint = objB.GlobalToLocal(end);
}
objB.RefinePath(path, this.m_expandedPath, ref startPoint, ref endPoint, num, num2);
int count = this.m_expandedPath.Count;
int num5 = this.m_expandedPath.Count;
while (true)
{
if (num5 >= count)
{
if (flag && !ReferenceEquals(group, objB))
{
this.m_expandedPath.Add(new Vector4D(vertex.WorldPosition, this.m_expandedPath[count - 1].W));
}
objB = group;
num = num3;
if (this.m_expandedPath.Count != 0)
{
startPoint = group.GlobalToLocal(new Vector3D(this.m_expandedPath[this.m_expandedPath.Count - 1]));
}
break;
}
Vector3D vectord = new Vector3D(this.m_expandedPath[num5]);
vectord = objB.LocalToGlobal((Vector3)vectord);
this.m_expandedPath[num5] = new Vector4D(vectord, this.m_expandedPath[num5].W);
num5++;
}
break;
}
num3++;
}
}
}
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();
}
}
}
}
}
