public void MarkBoxForAddition(BoundingBoxD box)
{
ProfilerShort.Begin("VoxelNavMesh.MarkBoxForAddition");
Vector3I pos, end;
MyVoxelCoordSystems.WorldPositionToVoxelCoord(m_voxelMap.PositionLeftBottomCorner, ref box.Min, out pos);
MyVoxelCoordSystems.WorldPositionToVoxelCoord(m_voxelMap.PositionLeftBottomCorner, ref box.Max, out end);
m_voxelMap.Storage.ClampVoxelCoord(ref pos);
m_voxelMap.Storage.ClampVoxelCoord(ref end);
MyVoxelCoordSystems.VoxelCoordToGeometryCellCoord(ref pos, out pos);
MyVoxelCoordSystems.VoxelCoordToGeometryCellCoord(ref end, out end);
Vector3 center = pos + end;
center = center * 0.5f;
pos /= 1 << NAVMESH_LOD;
end /= 1 << NAVMESH_LOD;
for (var it = new Vector3I.RangeIterator(ref pos, ref end); it.IsValid(); it.GetNext(out pos))
{
if (!m_processedCells.Contains(ref pos) && !m_markedForAddition.Contains(ref pos))
{
float weight = 1.0f / (0.01f + Vector3.RectangularDistance(pos, center));
if (!m_toAdd.Full)
{
m_toAdd.Insert(pos, weight);
m_markedForAddition.Add(ref pos);
}
else
{
float min = m_toAdd.MinKey();
if (weight > min)
{
Vector3I posRemoved = m_toAdd.RemoveMin();
m_markedForAddition.Remove(ref posRemoved);
m_toAdd.Insert(pos, weight);
m_markedForAddition.Add(ref pos);
}
}
}
}
ProfilerShort.End();
}
// Actually, this function marks even cubes around the block to make sure that any changes caused in their triangles
// will be reflected in the navigation mesh.
public void MarkBlockChanged(MySlimBlock block)
{
Vector3I min = block.Min - Vector3I.One;
Vector3I max = block.Max + Vector3I.One;
Vector3I pos = min;
for (var it = new Vector3I.RangeIterator(ref block.Min, ref block.Max); it.IsValid(); it.GetNext(out pos))
{
m_changedCubes.Add(pos);
}
Vector3I minCell = CubeToCell(ref min);
Vector3I maxCell = CubeToCell(ref max);
pos = minCell;
for (var it = new Vector3I.RangeIterator(ref minCell, ref maxCell); it.IsValid(); it.GetNext(out pos))
{
m_changedCells.Add(pos);
MyCellCoord cellCoord = new MyCellCoord(0, pos);
ulong packedCell = cellCoord.PackId64();
TryClearCell(packedCell);
}
}
// Actually, this function marks even cubes around the block to make sure that any changes caused in their triangles
// will be reflected in the navigation mesh.
public void MarkBlockChanged(MySlimBlock block)
{
Vector3I min = block.Min - Vector3I.One;
Vector3I max = block.Max + Vector3I.One;
Vector3I pos = min;
for (var it = new Vector3I_RangeIterator(ref block.Min, ref block.Max); it.IsValid(); it.GetNext(out pos))
{
m_changedCubes.Add(pos);
}
Vector3I minCell = CubeToCell(ref min);
Vector3I maxCell = CubeToCell(ref max);
pos = minCell;
for (var it = new Vector3I_RangeIterator(ref minCell, ref maxCell); it.IsValid(); it.GetNext(out pos))
{
m_changedCells.Add(pos);
}
}
private void OnBlockAddedInternal(MySlimBlock block)
{
var existingBlock = m_grid.GetCubeBlock(block.Position);
var compound = existingBlock.FatBlock as MyCompoundCubeBlock;
// Ignore blocks without navigation info (i.e. decorations and such)
if (!(block.FatBlock is MyCompoundCubeBlock) && block.BlockDefinition.NavigationDefinition == null)
{
return;
}
bool noEntry = false;
bool meshFound = false;
if (compound != null)
{
var blocks = compound.GetBlocks();
if (blocks.Count == 0)
{
return;
}
foreach (var subBlock in blocks)
{
if (subBlock.BlockDefinition.NavigationDefinition == null)
{
continue;
}
if (subBlock.BlockDefinition.NavigationDefinition.NoEntry || meshFound)
{
meshFound = false;
noEntry = true;
break;
}
else
{
block = subBlock;
meshFound = true;
}
}
}
else
{
if (block.BlockDefinition.NavigationDefinition != null)
{
if (block.BlockDefinition.NavigationDefinition.NoEntry)
{
meshFound = false;
noEntry = true;
}
else
{
meshFound = true;
}
}
}
// Ignore compounds with blocks without navigation info
if (!noEntry && !meshFound)
{
return;
}
if (noEntry)
{
if (m_cubeSet.Contains(block.Position))
{
RemoveBlock(block.Min, block.Max, true);
}
Vector3I pos = default(Vector3I);
for (pos.X = block.Min.X; pos.X <= block.Max.X; ++pos.X)
{
for (pos.Y = block.Min.Y; pos.Y <= block.Max.Y; ++pos.Y)
{
pos.Z = block.Min.Z - 1;
if (m_cubeSet.Contains(ref pos))
{
EraseFaceTriangles(pos, Base6Directions.Direction.Backward);
}
pos.Z = block.Max.Z + 1;
if (m_cubeSet.Contains(ref pos))
{
EraseFaceTriangles(pos, Base6Directions.Direction.Forward);
}
}
for (pos.Z = block.Min.Z; pos.Z <= block.Max.Z; ++pos.Z)
{
pos.Y = block.Min.Y - 1;
if (m_cubeSet.Contains(ref pos))
{
EraseFaceTriangles(pos, Base6Directions.Direction.Up);
}
pos.Y = block.Max.Y + 1;
if (m_cubeSet.Contains(ref pos))
{
EraseFaceTriangles(pos, Base6Directions.Direction.Down);
}
}
}
for (pos.Y = block.Min.Y; pos.Y <= block.Max.Y; ++pos.Y)
{
for (pos.Z = block.Min.Z; pos.Z <= block.Max.Z; ++pos.Z)
{
pos.X = block.Min.X - 1;
if (m_cubeSet.Contains(ref pos))
{
EraseFaceTriangles(pos, Base6Directions.Direction.Right);
}
pos.X = block.Max.X + 1;
if (m_cubeSet.Contains(ref pos))
{
EraseFaceTriangles(pos, Base6Directions.Direction.Left);
}
}
}
pos = block.Min;
for (var it = new Vector3I.RangeIterator(ref pos, ref block.Max); it.IsValid(); it.GetNext(out pos))
{
m_cubeSet.Add(pos);
}
}
else
{
if (m_cubeSet.Contains(block.Position))
{
RemoveBlock(block.Min, block.Max, eraseCubeSet: true);
}
AddBlock(block);
}
BoundingBoxD bbox;
block.GetWorldBoundingBox(out bbox);
bbox.Inflate(5.1f);
m_coordinator.InvalidateVoxelsBBox(ref bbox);
MarkBlockChanged(block);
}
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);
}
public void AddExplored(ref Vector3I cellPos)
{
m_exploredCells.Add(ref cellPos);
}
