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