private void CalculateSubtrees()
{
MyUnionFind uf = new MyUnionFind(GrowthSteps.Length);
for (int i = 0; i < GrowthSteps.Length; ++i)
{
var next = GrowthSteps[i].NextStep;
if (next != -1)
{
uf.Union(i, next);
}
}
/**
* To calculate lineages:
* - Find the zero steps
* - Calculate a subtree for each.
* - There is at most one cycle per connected component
* - We follow any node and determine the cycle.
* - Wach sequential lineage ends where it meets the cycle, we store that index
*
*/
// Lists of growth subtrees
Dictionary <int, MyTuple <List <int>, GrowthSubtree> > growthLists = new Dictionary <int, MyTuple <List <int>, GrowthSubtree> >();
for (int i = 0; i < GrowthSteps.Length; ++i)
{
var representant = uf.Find(i);
MyTuple <List <int>, GrowthSubtree> subtree;
if (!growthLists.TryGetValue(representant, out subtree))
{
subtree = new MyTuple <List <int>, GrowthSubtree>(new List <int>(), new GrowthSubtree());
growthLists.Add(representant, subtree);
;
m_subtrees.Add(subtree.Item2);
GrowthSteps[i].Subtree = subtree.Item2;
}
subtree.Item1.Add(i);
GrowthSteps[i].Subtree = subtree.Item2;
}
m_componentIndex = new int[GrowthSteps.Length];
foreach (var subtree in growthLists.Values)
{
subtree.Item2.Init(this, subtree.Item1);
}
}
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);
}