public void UpdateVoxelNavmeshCellHighLevelLinks(MyVoxelPathfinding.CellId cellId) { // Make sure links are where they should be List <MyNavigationPrimitive> linkedTriangles = null; if (m_voxelLinkDictionary.TryGetValue(cellId, out linkedTriangles)) { MyNavigationPrimitive hlPrimitive1 = null; MyNavigationPrimitive hlPrimitive2 = null; foreach (var primitive in linkedTriangles) { hlPrimitive1 = primitive.GetHighLevelPrimitive(); List <MyNavigationPrimitive> otherLinkedPrimitives = null; otherLinkedPrimitives = m_links.GetLinks(primitive); if (otherLinkedPrimitives != null) { foreach (var otherPrimitive in otherLinkedPrimitives) { hlPrimitive2 = otherPrimitive.GetHighLevelPrimitive(); m_highLevelLinks.AddLink(hlPrimitive1, hlPrimitive2, onlyIfNotPresent: true); } } } } // CH: TODO: Make sure that links are not where they should not be }
public void UpdateVoxelNavmeshCellHighLevelLinks(MyVoxelPathfinding.CellId cellId) { List <MyNavigationPrimitive> list = null; if (this.m_voxelLinkDictionary.TryGetValue(cellId, out list)) { MyNavigationPrimitive highLevelPrimitive = null; MyNavigationPrimitive highLevelPrimitive = null; foreach (MyNavigationPrimitive primitive3 in list) { highLevelPrimitive = primitive3.GetHighLevelPrimitive(); List <MyNavigationPrimitive> links = null; links = this.m_links.GetLinks(primitive3); if (links != null) { using (List <MyNavigationPrimitive> .Enumerator enumerator2 = links.GetEnumerator()) { while (enumerator2.MoveNext()) { highLevelPrimitive = enumerator2.Current.GetHighLevelPrimitive(); this.m_highLevelLinks.AddLink(highLevelPrimitive, highLevelPrimitive, true); } } } } } }
public void TryAddVoxelNavmeshLinks2(MyVoxelPathfinding.CellId cellId, Dictionary <MyGridPathfinding.CubeId, List <MyNavigationPrimitive> > linkCandidates) { ProfilerShort.Begin("TryAddVoxelNavmeshLinks"); foreach (var entry in linkCandidates) { double closestDistSq = double.MaxValue; MyNavigationTriangle closestGridTri = null; MyNavigationPrimitive closestLinkedPrim = null; m_tmpNavTris.Clear(); m_gridPathfinding.GetCubeTriangles(entry.Key, m_tmpNavTris); foreach (var tri in m_tmpNavTris) { Vector3 a, b, c; tri.GetVertices(out a, out b, out c); a = tri.Parent.LocalToGlobal(a); b = tri.Parent.LocalToGlobal(b); c = tri.Parent.LocalToGlobal(c); Vector3D normal = (c - a).Cross(b - a); Vector3D center = (a + b + c) / 3.0f; double lowerY = Math.Min(a.Y, Math.Min(b.Y, c.Y)); double upperY = Math.Max(a.Y, Math.Max(b.Y, c.Y)); lowerY -= 0.25f; upperY += 0.25f; foreach (var primitive in entry.Value) { Vector3D primPos = primitive.WorldPosition; Vector3D offset = primPos - center; double offsetLen = offset.Length(); offset = offset / offsetLen; double dot; Vector3D.Dot(ref offset, ref normal, out dot); if (dot > -0.2f && primPos.Y < upperY && primPos.Y > lowerY) { double dist = offsetLen / (dot + 0.3f); if (dist < closestDistSq) { closestDistSq = dist; closestGridTri = tri; closestLinkedPrim = primitive; } } } } m_tmpNavTris.Clear(); if (closestGridTri != null) { Debug.Assert(closestLinkedPrim.GetHighLevelPrimitive() != null); Debug.Assert(closestGridTri.GetHighLevelPrimitive() != null); m_links.AddLink(closestLinkedPrim, closestGridTri); SaveVoxelLinkToDictionary(cellId, closestLinkedPrim); IncreaseGridLinkCounter(entry.Key); } } ProfilerShort.End(); }
public void RemoveVoxelNavmeshLinks(MyVoxelPathfinding.CellId cellId) { List <MyNavigationPrimitive> list = null; if (this.m_voxelLinkDictionary.TryGetValue(cellId, out list)) { foreach (MyNavigationPrimitive primitive in list) { this.m_links.RemoveAllLinks(primitive); } this.m_voxelLinkDictionary.Remove(cellId); } }
private void RemoveVoxelLinkFromDictionary(MyVoxelPathfinding.CellId cellId, MyNavigationPrimitive linkedPrimitive) { List <MyNavigationPrimitive> list = null; if (this.m_voxelLinkDictionary.TryGetValue(cellId, out list)) { list.Remove(linkedPrimitive); if (list.Count == 0) { this.m_voxelLinkDictionary.Remove(cellId); } } }
public void TryAddVoxelNavmeshLinks2(MyVoxelPathfinding.CellId cellId, Dictionary <MyGridPathfinding.CubeId, List <MyNavigationPrimitive> > linkCandidates) { foreach (KeyValuePair <MyGridPathfinding.CubeId, List <MyNavigationPrimitive> > pair in linkCandidates) { double maxValue = double.MaxValue; MyNavigationTriangle triangle = null; MyNavigationPrimitive primitive = null; m_tmpNavTris.Clear(); this.m_gridPathfinding.GetCubeTriangles(pair.Key, m_tmpNavTris); foreach (MyNavigationTriangle triangle2 in m_tmpNavTris) { Vector3 vector; Vector3 vector2; Vector3 vector3; triangle2.GetVertices(out vector, out vector2, out vector3); vector = (Vector3)triangle2.Parent.LocalToGlobal(vector); vector2 = (Vector3)triangle2.Parent.LocalToGlobal(vector2); vector3 = (Vector3)triangle2.Parent.LocalToGlobal(vector3); Vector3D vectord = (vector3 - vector).Cross(vector2 - vector); Vector3D vectord2 = ((vector + vector2) + vector3) / 3f; double num2 = Math.Min(vector.Y, Math.Min(vector2.Y, vector3.Y)) - 0.25; double num3 = Math.Max(vector.Y, Math.Max(vector2.Y, vector3.Y)) + 0.25; foreach (MyNavigationPrimitive primitive2 in pair.Value) { double num5; Vector3D worldPosition = primitive2.WorldPosition; Vector3D vectord4 = worldPosition - vectord2; double num4 = vectord4.Length(); Vector3D.Dot(ref vectord4 / num4, ref vectord, out num5); if ((num5 > -0.20000000298023224) && ((worldPosition.Y < num3) && (worldPosition.Y > num2))) { double num6 = num4 / (num5 + 0.30000001192092896); if (num6 < maxValue) { maxValue = num6; triangle = triangle2; primitive = primitive2; } } } } m_tmpNavTris.Clear(); if (triangle != null) { this.m_links.AddLink(primitive, triangle, false); this.SaveVoxelLinkToDictionary(cellId, primitive); this.IncreaseGridLinkCounter(pair.Key); } } }
private void SaveVoxelLinkToDictionary(MyVoxelPathfinding.CellId cellId, MyNavigationPrimitive linkedPrimitive) { List <MyNavigationPrimitive> list = null; if (!this.m_voxelLinkDictionary.TryGetValue(cellId, out list)) { list = new List <MyNavigationPrimitive>(); } else if (list.Contains(linkedPrimitive)) { return; } list.Add(linkedPrimitive); this.m_voxelLinkDictionary[cellId] = list; }
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 bool RemoveCell(Vector3I cell) { if (!MyFakes.REMOVE_VOXEL_NAVMESH_CELLS) { return(true); } Debug.Assert(m_processedCells.Contains(cell), "Removing a non-existent cell from the navmesh!"); if (!m_processedCells.Contains(cell)) { return(false); } MyTrace.Send(TraceWindow.Ai, "Removing cell " + cell); ProfilerShort.Begin("Removing navmesh links"); MyVoxelPathfinding.CellId cellId = new MyVoxelPathfinding.CellId() { VoxelMap = m_voxelMap, Pos = cell }; m_navmeshCoordinator.RemoveVoxelNavmeshLinks(cellId); ProfilerShort.End(); ProfilerShort.Begin("Removing triangles"); MyCellCoord coord = new MyCellCoord(NAVMESH_LOD, cell); ulong packedCoord = coord.PackId64(); MyIntervalList triangleList = m_higherLevelHelper.TryGetTriangleList(packedCoord); if (triangleList != null) { foreach (var triangleIndex in triangleList) { RemoveTerrainTriangle(GetTriangle(triangleIndex)); } m_higherLevelHelper.ClearCachedCell(packedCoord); } ProfilerShort.End(); Debug.Assert(m_processedCells.Contains(ref cell)); m_processedCells.Remove(ref cell); return(triangleList != null); }
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 bool RemoveCell(Vector3I cell) { if (!MyFakes.REMOVE_VOXEL_NAVMESH_CELLS) { return(true); } if (!this.m_processedCells.Contains(cell)) { return(false); } if (MyFakes.LOG_NAVMESH_GENERATION) { MyCestmirPathfindingShorts.Pathfinding.VoxelPathfinding.DebugLog.LogCellRemoval(this, cell); } MyVoxelPathfinding.CellId cellId = new MyVoxelPathfinding.CellId { VoxelMap = this.m_voxelMap, Pos = cell }; this.m_navmeshCoordinator.RemoveVoxelNavmeshLinks(cellId); ulong packedCellCoord = new MyCellCoord(0, cell).PackId64(); MyIntervalList list = this.m_higherLevelHelper.TryGetTriangleList(packedCellCoord); if (list != null) { MyIntervalList.Enumerator enumerator = list.GetEnumerator(); while (true) { if (!enumerator.MoveNext()) { this.m_higherLevelHelper.ClearCachedCell(packedCellCoord); break; } int current = enumerator.Current; this.RemoveTerrainTriangle(base.GetTriangle(current)); } } this.m_processedCells.Remove(ref cell); return(list != null); }
private bool RemoveCell(Vector3I cell) { if (!MyFakes.REMOVE_VOXEL_NAVMESH_CELLS) return true; Debug.Assert(m_processedCells.Contains(cell), "Removing a non-existent cell from the navmesh!"); if (!m_processedCells.Contains(cell)) return false; MyTrace.Send(TraceWindow.Ai, "Removing cell " + cell); ProfilerShort.Begin("Removing navmesh links"); MyVoxelPathfinding.CellId cellId = new MyVoxelPathfinding.CellId() { VoxelMap = m_voxelMap, Pos = cell }; m_navmeshCoordinator.RemoveVoxelNavmeshLinks(cellId); ProfilerShort.End(); ProfilerShort.Begin("Removing triangles"); MyCellCoord coord = new MyCellCoord(NAVMESH_LOD, cell); ulong packedCoord = coord.PackId64(); MyIntervalList triangleList = m_higherLevelHelper.TryGetTriangleList(packedCoord); if (triangleList != null) { foreach (var triangleIndex in triangleList) { RemoveTerrainTriangle(GetTriangle(triangleIndex)); } m_higherLevelHelper.ClearCachedCell(packedCoord); } ProfilerShort.End(); Debug.Assert(m_processedCells.Contains(ref cell)); m_processedCells.Remove(ref cell); return triangleList != null; }
private bool AddCell(Vector3I cellPos) { MyCellCoord coord = new MyCellCoord(NAVMESH_LOD, cellPos); var generatedMesh = MyPrecalcComponent.IsoMesher.Precalc(new MyIsoMesherArgs() { Storage = m_voxelMap.Storage, GeometryCell = coord, }); if (generatedMesh == null) { m_processedCells.Add(ref cellPos); m_higherLevelHelper.AddExplored(ref cellPos); return false; } ulong packedCoord = coord.PackId64(); List<DebugDrawEdge> debugEdgesList = new List<DebugDrawEdge>(); m_debugCellEdges[packedCoord] = debugEdgesList; MyVoxelPathfinding.CellId cellId = new MyVoxelPathfinding.CellId() { VoxelMap = m_voxelMap, Pos = cellPos }; MyTrace.Send(TraceWindow.Ai, "Adding cell " + cellPos); m_connectionHelper.ClearCell(); m_vertexMapping.Init(generatedMesh.VerticesCount); // Prepare list of possibly intersecting cube grids for voxel-grid navmesh intersection testing Vector3D bbMin = m_voxelMap.PositionLeftBottomCorner + (m_cellSize * (new Vector3D(-0.125) + cellPos)); Vector3D bbMax = m_voxelMap.PositionLeftBottomCorner + (m_cellSize * (Vector3D.One + cellPos)); BoundingBoxD cellBB = new BoundingBoxD(bbMin, bbMax); m_tmpGridList.Clear(); m_navmeshCoordinator.PrepareVoxelTriangleTests(cellBB, m_tmpGridList); Vector3D voxelMapCenter = m_voxelMap.PositionComp.GetPosition(); Vector3 centerDisplacement = voxelMapCenter - m_voxelMap.PositionLeftBottomCorner; // This is needed for correct edge classification - to tell, whether the edges are inner or outer edges of the cell ProfilerShort.Begin("Triangle preprocessing"); for (int i = 0; i < generatedMesh.TrianglesCount; i++) { ushort a = generatedMesh.Triangles[i].VertexIndex0; ushort b = generatedMesh.Triangles[i].VertexIndex1; ushort c = generatedMesh.Triangles[i].VertexIndex2; Vector3 aPos, bPos, cPos; Vector3 vert; generatedMesh.GetUnpackedPosition(a, out vert); aPos = vert - centerDisplacement; generatedMesh.GetUnpackedPosition(b, out vert); bPos = vert - centerDisplacement; generatedMesh.GetUnpackedPosition(c, out vert); cPos = vert - centerDisplacement; bool invalidTriangle = false; if ((bPos - aPos).LengthSquared() <= MyVoxelConnectionHelper.OUTER_EDGE_EPSILON_SQ) { m_vertexMapping.Union(a, b); invalidTriangle = true; } if ((cPos - aPos).LengthSquared() <= MyVoxelConnectionHelper.OUTER_EDGE_EPSILON_SQ) { m_vertexMapping.Union(a, c); invalidTriangle = true; } if ((cPos - bPos).LengthSquared() <= MyVoxelConnectionHelper.OUTER_EDGE_EPSILON_SQ) { m_vertexMapping.Union(b, c); invalidTriangle = true; } if (invalidTriangle) continue; m_connectionHelper.PreprocessInnerEdge(a, b); m_connectionHelper.PreprocessInnerEdge(b, c); m_connectionHelper.PreprocessInnerEdge(c, a); } ProfilerShort.End(); ProfilerShort.Begin("Free face sorting"); // Ensure that the faces have increasing index numbers Mesh.SortFreeFaces(); ProfilerShort.End(); m_higherLevelHelper.OpenNewCell(coord); ProfilerShort.Begin("Adding triangles"); for (int i = 0; i < generatedMesh.TrianglesCount; i++) { ushort a = generatedMesh.Triangles[i].VertexIndex0; ushort b = generatedMesh.Triangles[i].VertexIndex1; ushort c = generatedMesh.Triangles[i].VertexIndex2; ushort setA = (ushort)m_vertexMapping.Find(a); ushort setB = (ushort)m_vertexMapping.Find(b); ushort setC = (ushort)m_vertexMapping.Find(c); if (setA == setB || setB == setC || setA == setC) continue; Vector3 aPos, bPos, cPos; Vector3 vert; generatedMesh.GetUnpackedPosition(a, out vert); aPos = vert - centerDisplacement; generatedMesh.GetUnpackedPosition(b, out vert); bPos = vert - centerDisplacement; generatedMesh.GetUnpackedPosition(c, out vert); cPos = vert - centerDisplacement; if (MyPerGameSettings.NavmeshPresumesDownwardGravity) { Vector3 normal = (cPos - aPos).Cross(bPos - aPos); normal.Normalize(); if (normal.Dot(ref Vector3.Up) <= Math.Cos(MathHelper.ToRadians(54.0f))) continue; } Vector3D aTformed = aPos + voxelMapCenter; Vector3D bTformed = bPos + voxelMapCenter; Vector3D cTformed = cPos + voxelMapCenter; bool intersecting = false; m_tmpLinkCandidates.Clear(); m_navmeshCoordinator.TestVoxelNavmeshTriangle(ref aTformed, ref bTformed, ref cTformed, m_tmpGridList, m_tmpLinkCandidates, out intersecting); if (intersecting) { m_tmpLinkCandidates.Clear(); continue; } if (!m_connectionHelper.IsInnerEdge(a, b)) debugEdgesList.Add(new DebugDrawEdge(aTformed, bTformed)); if (!m_connectionHelper.IsInnerEdge(b, c)) debugEdgesList.Add(new DebugDrawEdge(bTformed, cTformed)); if (!m_connectionHelper.IsInnerEdge(c, a)) debugEdgesList.Add(new DebugDrawEdge(cTformed, aTformed)); int edgeAB = m_connectionHelper.TryGetAndRemoveEdgeIndex(b, a, ref bPos, ref aPos); int edgeBC = m_connectionHelper.TryGetAndRemoveEdgeIndex(c, b, ref cPos, ref bPos); int edgeCA = m_connectionHelper.TryGetAndRemoveEdgeIndex(a, c, ref aPos, ref cPos); int formerAB = edgeAB; int formerBC = edgeBC; int formerCA = edgeCA; ProfilerShort.Begin("AddTriangle"); var tri = AddTriangle(ref aPos, ref bPos, ref cPos, ref edgeAB, ref edgeBC, ref edgeCA); ProfilerShort.End(); CheckMeshConsistency(); m_higherLevelHelper.AddTriangle(tri.Index); if (formerAB == -1) m_connectionHelper.AddEdgeIndex(a, b, ref aPos, ref bPos, edgeAB); if (formerBC == -1) m_connectionHelper.AddEdgeIndex(b, c, ref bPos, ref cPos, edgeBC); if (formerCA == -1) m_connectionHelper.AddEdgeIndex(c, a, ref cPos, ref aPos, edgeCA); // TODO: Instead of this, just add the tri into a list of tris that want to connect with the link candidates //m_navmeshCoordinator.TryAddVoxelNavmeshLinks(tri, cellId, m_tmpLinkCandidates); foreach (var candidate in m_tmpLinkCandidates) { List<MyNavigationPrimitive> primitives = null; if (!m_tmpCubeLinkCandidates.TryGetValue(candidate, out primitives)) { primitives = m_primitiveListPool.Allocate(); m_tmpCubeLinkCandidates.Add(candidate, primitives); } primitives.Add(tri); } m_tmpLinkCandidates.Clear(); } ProfilerShort.End(); m_tmpGridList.Clear(); m_connectionHelper.ClearCell(); m_vertexMapping.Clear(); Debug.Assert(!m_processedCells.Contains(ref cellPos)); m_processedCells.Add(ref cellPos); m_higherLevelHelper.AddExplored(ref cellPos); // Find connected components in the current cell's subgraph of the navigation mesh m_higherLevelHelper.ProcessCellComponents(); m_higherLevelHelper.CloseCell(); // Create navmesh links using the navmesh coordinator, taking into consideration the high level components m_navmeshCoordinator.TryAddVoxelNavmeshLinks2(cellId, m_tmpCubeLinkCandidates); m_navmeshCoordinator.UpdateVoxelNavmeshCellHighLevelLinks(cellId); foreach (var candidate in m_tmpCubeLinkCandidates) { candidate.Value.Clear(); m_primitiveListPool.Deallocate(candidate.Value); } m_tmpCubeLinkCandidates.Clear(); return true; }
public void TryAddVoxelNavmeshLinks(MyNavigationTriangle addedPrimitive, MyVoxelPathfinding.CellId cellId, List <MyGridPathfinding.CubeId> linkCandidates) { m_tmpNavTris.Clear(); using (List <MyGridPathfinding.CubeId> .Enumerator enumerator = linkCandidates.GetEnumerator()) { MyGridPathfinding.CubeId current; MyNavigationTriangle triangle; bool flag; goto TR_0021; TR_0002: m_tmpNavTris.Clear(); goto TR_0021; TR_0004: if (flag) { this.m_links.AddLink(addedPrimitive, triangle, false); this.SaveVoxelLinkToDictionary(cellId, addedPrimitive); this.IncreaseGridLinkCounter(current); } goto TR_0002; TR_0005: m_tmpNavPrims.Clear(); goto TR_0004; TR_0021: while (true) { if (enumerator.MoveNext()) { current = enumerator.Current; this.m_gridPathfinding.GetCubeTriangles(current, m_tmpNavTris); double maxValue = double.MaxValue; triangle = null; foreach (MyNavigationTriangle triangle2 in m_tmpNavTris) { Vector3D vectord = addedPrimitive.WorldPosition - triangle2.WorldPosition; if (MyPerGameSettings.NavmeshPresumesDownwardGravity && ((Math.Abs(vectord.Y) < 0.3) && (vectord.LengthSquared() < maxValue))) { maxValue = vectord.LengthSquared(); triangle = triangle2; } } if (triangle == null) { goto TR_0002; } else { flag = true; List <MyNavigationPrimitive> links = this.m_links.GetLinks(triangle); List <MyNavigationPrimitive> list2 = null; this.m_voxelLinkDictionary.TryGetValue(cellId, out list2); if (links == null) { goto TR_0004; } else { m_tmpNavPrims.Clear(); this.CollectClosePrimitives(addedPrimitive, m_tmpNavPrims, 2); for (int i = 0; i < m_tmpNavPrims.Count; i++) { if ((links.Contains(m_tmpNavPrims[i]) && (list2 != null)) && list2.Contains(m_tmpNavPrims[i])) { if ((m_tmpNavPrims[i].WorldPosition - triangle.WorldPosition).LengthSquared() < maxValue) { flag = false; break; } this.m_links.RemoveLink(triangle, m_tmpNavPrims[i]); if (this.m_links.GetLinkCount(m_tmpNavPrims[i]) == 0) { this.RemoveVoxelLinkFromDictionary(cellId, m_tmpNavPrims[i]); } this.DecreaseGridLinkCounter(current); } } } } } else { return; } break; } goto TR_0005; } }
private bool AddCell(Vector3I cellPos) { MyCellCoord coord = new MyCellCoord(NAVMESH_LOD, cellPos); var geometry = m_voxelMap.Storage.Geometry; MyVoxelGeometry.CellData data = geometry.GetCell(ref coord); if (data == null) { m_processedCells.Add(ref cellPos); m_higherLevelHelper.AddExplored(ref cellPos); return(false); } ulong packedCoord = coord.PackId64(); List <DebugDrawEdge> debugEdgesList = new List <DebugDrawEdge>(); m_debugCellEdges[packedCoord] = debugEdgesList; MyVoxelPathfinding.CellId cellId = new MyVoxelPathfinding.CellId() { VoxelMap = m_voxelMap, Pos = cellPos }; MyTrace.Send(TraceWindow.Ai, "Adding cell " + cellPos); m_connectionHelper.ClearCell(); m_vertexMapping.Init(data.VoxelVerticesCount); // Prepare list of possibly intersecting cube grids for voxel-grid navmesh intersection testing Vector3D bbMin = m_voxelMap.PositionLeftBottomCorner + (m_cellSize * (new Vector3D(-0.125) + cellPos)); Vector3D bbMax = m_voxelMap.PositionLeftBottomCorner + (m_cellSize * (Vector3D.One + cellPos)); BoundingBoxD cellBB = new BoundingBoxD(bbMin, bbMax); m_tmpGridList.Clear(); m_navmeshCoordinator.PrepareVoxelTriangleTests(cellBB, m_tmpGridList); Vector3D voxelMapCenter = m_voxelMap.PositionComp.GetPosition(); Vector3 centerDisplacement = voxelMapCenter - m_voxelMap.PositionLeftBottomCorner; // This is needed for correct edge classification - to tell, whether the edges are inner or outer edges of the cell ProfilerShort.Begin("Triangle preprocessing"); for (int i = 0; i < data.VoxelTrianglesCount; i++) { short a = data.VoxelTriangles[i].VertexIndex0; short b = data.VoxelTriangles[i].VertexIndex1; short c = data.VoxelTriangles[i].VertexIndex2; Vector3 aPos, bPos, cPos; Vector3 vert; data.GetUnpackedPosition(a, out vert); aPos = vert - centerDisplacement; data.GetUnpackedPosition(b, out vert); bPos = vert - centerDisplacement; data.GetUnpackedPosition(c, out vert); cPos = vert - centerDisplacement; bool invalidTriangle = false; if ((bPos - aPos).LengthSquared() <= MyVoxelConnectionHelper.OUTER_EDGE_EPSILON_SQ) { m_vertexMapping.Union(a, b); invalidTriangle = true; } if ((cPos - aPos).LengthSquared() <= MyVoxelConnectionHelper.OUTER_EDGE_EPSILON_SQ) { m_vertexMapping.Union(a, c); invalidTriangle = true; } if ((cPos - bPos).LengthSquared() <= MyVoxelConnectionHelper.OUTER_EDGE_EPSILON_SQ) { m_vertexMapping.Union(b, c); invalidTriangle = true; } if (invalidTriangle) { continue; } m_connectionHelper.PreprocessInnerEdge(a, b); m_connectionHelper.PreprocessInnerEdge(b, c); m_connectionHelper.PreprocessInnerEdge(c, a); } ProfilerShort.End(); ProfilerShort.Begin("Free face sorting"); // Ensure that the faces have increasing index numbers Mesh.SortFreeFaces(); ProfilerShort.End(); m_higherLevelHelper.OpenNewCell(coord); ProfilerShort.Begin("Adding triangles"); for (int i = 0; i < data.VoxelTrianglesCount; i++) { short a = data.VoxelTriangles[i].VertexIndex0; short b = data.VoxelTriangles[i].VertexIndex1; short c = data.VoxelTriangles[i].VertexIndex2; short setA = (short)m_vertexMapping.Find(a); short setB = (short)m_vertexMapping.Find(b); short setC = (short)m_vertexMapping.Find(c); if (setA == setB || setB == setC || setA == setC) { continue; } Vector3 aPos, bPos, cPos; Vector3 vert; data.GetUnpackedPosition(setA, out vert); aPos = vert - centerDisplacement; data.GetUnpackedPosition(setB, out vert); bPos = vert - centerDisplacement; data.GetUnpackedPosition(setC, out vert); cPos = vert - centerDisplacement; if (MyFakes.NAVMESH_PRESUMES_DOWNWARD_GRAVITY) { Vector3 normal = (cPos - aPos).Cross(bPos - aPos); normal.Normalize(); if (normal.Dot(ref Vector3.Up) <= Math.Cos(MathHelper.ToRadians(54.0f))) { continue; } } Vector3D aTformed = aPos + voxelMapCenter; Vector3D bTformed = bPos + voxelMapCenter; Vector3D cTformed = cPos + voxelMapCenter; bool intersecting = false; m_tmpLinkCandidates.Clear(); m_navmeshCoordinator.TestVoxelNavmeshTriangle(ref aTformed, ref bTformed, ref cTformed, m_tmpGridList, m_tmpLinkCandidates, out intersecting); if (intersecting) { m_tmpLinkCandidates.Clear(); continue; } if (!m_connectionHelper.IsInnerEdge(a, b)) { debugEdgesList.Add(new DebugDrawEdge(aTformed, bTformed)); } if (!m_connectionHelper.IsInnerEdge(b, c)) { debugEdgesList.Add(new DebugDrawEdge(bTformed, cTformed)); } if (!m_connectionHelper.IsInnerEdge(c, a)) { debugEdgesList.Add(new DebugDrawEdge(cTformed, aTformed)); } int edgeAB = m_connectionHelper.TryGetAndRemoveEdgeIndex(b, a, ref bPos, ref aPos); int edgeBC = m_connectionHelper.TryGetAndRemoveEdgeIndex(c, b, ref cPos, ref bPos); int edgeCA = m_connectionHelper.TryGetAndRemoveEdgeIndex(a, c, ref aPos, ref cPos); int formerAB = edgeAB; int formerBC = edgeBC; int formerCA = edgeCA; ProfilerShort.Begin("AddTriangle"); var tri = AddTriangle(ref aPos, ref bPos, ref cPos, ref edgeAB, ref edgeBC, ref edgeCA); ProfilerShort.End(); CheckMeshConsistency(); m_higherLevelHelper.AddTriangle(tri.Index); if (formerAB == -1) { m_connectionHelper.AddEdgeIndex(a, b, ref aPos, ref bPos, edgeAB); } if (formerBC == -1) { m_connectionHelper.AddEdgeIndex(b, c, ref bPos, ref cPos, edgeBC); } if (formerCA == -1) { m_connectionHelper.AddEdgeIndex(c, a, ref cPos, ref aPos, edgeCA); } // TODO: Instead of this, just add the tri into a list of tris that want to connect with the link candidates //m_navmeshCoordinator.TryAddVoxelNavmeshLinks(tri, cellId, m_tmpLinkCandidates); foreach (var candidate in m_tmpLinkCandidates) { List <MyNavigationPrimitive> primitives = null; if (!m_tmpCubeLinkCandidates.TryGetValue(candidate, out primitives)) { primitives = m_primitiveListPool.Allocate(); m_tmpCubeLinkCandidates.Add(candidate, primitives); } primitives.Add(tri); } m_tmpLinkCandidates.Clear(); } ProfilerShort.End(); m_tmpGridList.Clear(); m_connectionHelper.ClearCell(); m_vertexMapping.Clear(); Debug.Assert(!m_processedCells.Contains(ref cellPos)); m_processedCells.Add(ref cellPos); m_higherLevelHelper.AddExplored(ref cellPos); // Find connected components in the current cell's subgraph of the navigation mesh m_higherLevelHelper.ProcessCellComponents(); m_higherLevelHelper.CloseCell(); // Create navmesh links using the navmesh coordinator, taking into consideration the high level components m_navmeshCoordinator.TryAddVoxelNavmeshLinks2(cellId, m_tmpCubeLinkCandidates); m_navmeshCoordinator.UpdateVoxelNavmeshCellHighLevelLinks(cellId); foreach (var candidate in m_tmpCubeLinkCandidates) { candidate.Value.Clear(); m_primitiveListPool.Deallocate(candidate.Value); } m_tmpCubeLinkCandidates.Clear(); return(true); }
// This is an old version of the function public void TryAddVoxelNavmeshLinks(MyNavigationTriangle addedPrimitive, MyVoxelPathfinding.CellId cellId, List <MyGridPathfinding.CubeId> linkCandidates) { ProfilerShort.Begin("TryAddVoxelNavmeshLinks"); m_tmpNavTris.Clear(); foreach (var candidate in linkCandidates) { // First, find closest navigation triangle from the given candidate cube ProfilerShort.Begin("Find closest grid nav tri"); m_gridPathfinding.GetCubeTriangles(candidate, m_tmpNavTris); double closestDistSq = double.MaxValue; MyNavigationTriangle closestGridTri = null; foreach (var tri in m_tmpNavTris) { Vector3D posDiff = addedPrimitive.WorldPosition - tri.WorldPosition; if (MyPerGameSettings.NavmeshPresumesDownwardGravity) { if (Math.Abs(posDiff.Y) < 0.3) { if (posDiff.LengthSquared() < closestDistSq) { closestDistSq = posDiff.LengthSquared(); closestGridTri = tri; } } } } ProfilerShort.End(); if (closestGridTri != null) { bool createLink = true; var existingLinks = m_links.GetLinks(closestGridTri); List <MyNavigationPrimitive> existingCellLinks = null; m_voxelLinkDictionary.TryGetValue(cellId, out existingCellLinks); if (existingLinks != null) { m_tmpNavPrims.Clear(); CollectClosePrimitives(addedPrimitive, m_tmpNavPrims, 2); for (int i = 0; i < m_tmpNavPrims.Count; ++i) { if (existingLinks.Contains(m_tmpNavPrims[i]) && existingCellLinks != null && existingCellLinks.Contains(m_tmpNavPrims[i])) { double existingDistSq = (m_tmpNavPrims[i].WorldPosition - closestGridTri.WorldPosition).LengthSquared(); if (existingDistSq < closestDistSq) { createLink = false; break; } else { m_links.RemoveLink(closestGridTri, m_tmpNavPrims[i]); if (m_links.GetLinkCount(m_tmpNavPrims[i]) == 0) { RemoveVoxelLinkFromDictionary(cellId, m_tmpNavPrims[i]); } DecreaseGridLinkCounter(candidate); continue; } } } m_tmpNavPrims.Clear(); } if (createLink) { m_links.AddLink(addedPrimitive, closestGridTri); SaveVoxelLinkToDictionary(cellId, addedPrimitive); IncreaseGridLinkCounter(candidate); } } m_tmpNavTris.Clear(); } ProfilerShort.End(); }