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(); }