bool IMyHighLevelComponent.Contains(MyNavigationPrimitive primitive)
{
MyNavigationTriangle tri = primitive as MyNavigationTriangle;
if (tri == null) return false;
return tri.ComponentIndex == m_componentIndex;
}
public bool Contains(MyNavigationPrimitive primitive)
{
if (primitive.Group != m_parent) return false;
var triangle = primitive as MyNavigationTriangle;
if (triangle == null) return false;
return triangle.ComponentIndex == m_componentIndex;
}
public void Init(Vector3D start, MySmartGoal goal)
{
ProfilerShort.Begin("MySmartPath.Init()");
Debug.Assert(m_valid == false);
m_lastInitTime = MySandboxGame.TotalGamePlayTimeInMilliseconds;
m_startPoint = start;
m_goal = goal;
ProfilerShort.Begin("Find start primitive");
m_currentPrimitive = m_pathfinding.FindClosestPrimitive(start, highLevel: false);
if (m_currentPrimitive != null)
{
m_hlBegin = m_currentPrimitive.GetHighLevelPrimitive();
Debug.Assert(m_hlBegin != null, "Start primitive did not have a high-level primitive!");
if (m_hlBegin != null && !m_pathNodes.Contains(m_hlBegin))
{
m_hlBegin.Parent.ObservePrimitive(m_hlBegin, this);
}
}
ProfilerShort.End();
if (m_currentPrimitive == null)
{
// CH: TODO: Starting primitive was not found. What to do now?
m_currentPrimitive = null;
Invalidate();
ProfilerShort.End();
return;
}
m_pathNodePosition = 0;
m_expandedPathPosition = 0;
m_expandedPath.Clear();
m_pathNodes.Clear();
m_usedWholePath = false;
m_valid = true;
ProfilerShort.End();
}
private void RefineFoundPath(ref Vector3D begin, ref Vector3D end, MyPath<MyNavigationPrimitive> path)
{
Debug.Assert(MyPerGameSettings.EnablePathfinding, "Pathfinding is not enabled!");
if (!MyPerGameSettings.EnablePathfinding)
{
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();
}
public void Invalidate()
{
if (m_valid == false)
{
return;
}
ClearPathNodes();
m_expandedPath.Clear();
m_expandedPathPosition = 0;
m_currentPrimitive = null;
if (m_goal.IsValid)
{
m_goal.Invalidate();
}
if (m_hlBegin != null)
{
m_hlBegin.Parent.StopObservingPrimitive(m_hlBegin, this);
}
m_hlBegin = null;
m_valid = false;
}
public void Reinit(Vector3D newStart)
{
var previousGoal = m_goal;
var previousEntity = previousGoal.EndEntity;
ClearPathNodes();
m_expandedPath.Clear();
m_expandedPathPosition = 0;
m_currentPrimitive = null;
if (m_hlBegin != null)
{
m_hlBegin.Parent.StopObservingPrimitive(m_hlBegin, this);
}
m_hlBegin = null;
m_valid = false;
m_goal.Reinit();
Init(newStart, previousGoal);
}
public override MyHighLevelPrimitive GetHighLevelPrimitive(MyNavigationPrimitive myNavigationTriangle)
{
return m_higherLevelHelper.GetHighLevelNavigationPrimitive(myNavigationTriangle as MyNavigationTriangle);
}
private static bool ProcessTriangleForHierarchyStatic(MyNavigationPrimitive primitive)
{
ProfilerShort.Begin("ProcessTriangleForHierarchy");
var triangle = primitive as MyNavigationTriangle;
bool retval = m_currentHelper.ProcessTriangleForHierarchy(triangle);
ProfilerShort.End();
return retval;
}
private void CollectClosePrimitives(MyNavigationPrimitive addedPrimitive, List<MyNavigationPrimitive> output, int depth)
{
if (depth < 0) return;
ProfilerShort.Begin("CollectClosePrimitives");
int prevPrevDepthBegin = output.Count;
output.Add(addedPrimitive);
int prevDepthBegin = output.Count;
for (int i = 0; i < addedPrimitive.GetOwnNeighborCount(); ++i)
{
var neighbor = addedPrimitive.GetOwnNeighbor(i) as MyNavigationPrimitive;
if (neighbor != null)
output.Add(neighbor);
}
int prevDepthEnd = output.Count;
depth--;
while (depth > 0)
{
for (int p = prevDepthBegin; p < prevDepthEnd; ++p)
{
MyNavigationPrimitive primitive = output[p];
for (int i = 0; i < primitive.GetOwnNeighborCount(); ++i)
{
var neighbor = primitive.GetOwnNeighbor(i) as MyNavigationPrimitive;
bool alreadyAdded = false;
for (int j = prevPrevDepthBegin; j < prevDepthEnd; ++j)
{
if (output[j] == neighbor)
{
alreadyAdded = true;
break;
}
}
if (!alreadyAdded && neighbor != null)
output.Add(neighbor);
}
}
prevPrevDepthBegin = prevDepthBegin;
prevDepthBegin = prevDepthEnd;
prevDepthEnd = output.Count;
depth--;
}
ProfilerShort.End();
}
private void RemoveVoxelLinkFromDictionary(MyVoxelPathfinding.CellId cellId, MyNavigationPrimitive linkedPrimitive)
{
List<MyNavigationPrimitive> list = null;
if (!m_voxelLinkDictionary.TryGetValue(cellId, out list))
{
Debug.Assert(false, "Could not find a removed voxel link in the dictionary!");
return;
}
else
{
bool retval = list.Remove(linkedPrimitive);
Debug.Assert(retval == true, "Couldn't remove a linked triangle from the dictionary!");
if (list.Count == 0)
{
m_voxelLinkDictionary.Remove(cellId);
}
}
}
private void SaveVoxelLinkToDictionary(MyVoxelPathfinding.CellId cellId, MyNavigationPrimitive linkedPrimitive)
{
List<MyNavigationPrimitive> list = null;
if (!m_voxelLinkDictionary.TryGetValue(cellId, out list))
{
// CH:TODO: take these from pre-allocated pools
list = new List<MyNavigationPrimitive>();
}
else if (list.Contains(linkedPrimitive))
{
// Avoid duplicates
return;
}
list.Add(linkedPrimitive);
m_voxelLinkDictionary[cellId] = list;
}
private static float HlCriterion(MyNavigationPrimitive primitive)
{
var hlPrimitive = primitive as MyHighLevelPrimitive;
Debug.Assert(hlPrimitive != null, "Primitive in smart path termination criterion was not a high-level navigation primitive!");
if (hlPrimitive == null || m_pathfindingStatic.m_ignoredPrimitives.Contains(hlPrimitive))
{
return float.PositiveInfinity;
}
float dist = m_pathfindingStatic.m_destination.PointAdmissibility(primitive.WorldPosition, 8.7f); // ~= sqrt(3) * voxelCellSize/2
if (dist < float.PositiveInfinity)
{
return dist * 4.0f;
}
var component = hlPrimitive.GetComponent();
Debug.Assert(component != null, "Component was null in termination criterion of smart path!");
if (component == null)
{
return float.PositiveInfinity;
}
if (!component.FullyExplored)
{
return (float)Vector3D.RectangularDistance(primitive.WorldPosition, m_pathfindingStatic.m_destinationCenter) * 8.0f;
}
return float.PositiveInfinity;
}
private static float HlHeuristic(MyNavigationPrimitive primitive)
{
return (float)Vector3D.RectangularDistance(primitive.WorldPosition, m_pathfindingStatic.m_destinationCenter) * 2.0f;
}
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 float Heuristic(MyNavigationPrimitive primitive)
{
return (float)Vector3D.Distance(primitive.WorldPosition, m_destinationCenter);
}
public MyPath<MyNavigationPrimitive> FindPath(MyPathfinding pathfinding, MyNavigationPrimitive startPrimitive)
{
throw new NotImplementedException();
}
