public override void DebugDraw(ref Matrix drawMatrix)
{
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW)
{
if (((MyDebugDrawSettings.DEBUG_DRAW_NAVMESHES & MyWEMDebugDrawMode.EDGES) != MyWEMDebugDrawMode.NONE) && (this.m_connectionHelper != null))
{
foreach (KeyValuePair <EdgeIndex, int> pair in this.m_connectionHelper)
{
Vector3 pointTo = Vector3.Transform(pair.Key.B / 256f, (Matrix)drawMatrix);
MyRenderProxy.DebugDrawLine3D(Vector3.Transform(pair.Key.A / 256f, (Matrix)drawMatrix), pointTo, Color.Red, Color.Yellow, false, false);
}
}
if ((MyDebugDrawSettings.DEBUG_DRAW_NAVMESHES & MyWEMDebugDrawMode.NORMALS) != MyWEMDebugDrawMode.NONE)
{
foreach (KeyValuePair <Vector3I, List <int> > pair2 in this.m_smallTriangleRegistry)
{
foreach (int num in pair2.Value)
{
MyNavigationTriangle triangle = base.GetTriangle(num);
Vector3 pointTo = Vector3.Transform(triangle.Center + (triangle.Normal * 0.2f), (Matrix)drawMatrix);
MyRenderProxy.DebugDrawLine3D(Vector3.Transform(triangle.Center, (Matrix)drawMatrix), pointTo, Color.Blue, Color.Blue, true, false);
}
}
}
if (MyFakes.DEBUG_DRAW_NAVMESH_HIERARCHY && (this.m_higherLevel != null))
{
this.m_higherLevel.DebugDraw(MyFakes.DEBUG_DRAW_NAVMESH_HIERARCHY_LITE);
}
}
}
private bool ProcessTriangleForHierarchy(MyNavigationTriangle triangle)
{
// The triangle parent can be wrong when we have multiple navmeshes connected via external edges
if (triangle.Parent != m_mesh)
{
return(false);
}
if (m_tmpCellTriangles.Contains(triangle.Index))
{
m_components.AddComponentTriangle(triangle, triangle.Center);
m_tmpComponentTriangles.Add(triangle);
return(true);
}
else
{
ulong cellIndex;
// This test succeeds only if the triangle belongs to an unchanged component or to a component that was changed,
// but processed in a different cell already
if (m_components.TryGetComponentCell(triangle.ComponentIndex, out cellIndex))
{
// Save connections to other components. There won't be so many, so we can keep them in a list instead of a HashSet
if (!m_currentCellConnections[m_currentComponentRel].Contains(triangle.ComponentIndex))
{
m_currentCellConnections[m_currentComponentRel].Add(triangle.ComponentIndex);
}
}
}
return(false);
}
/// <summary>
/// All coords should be in the voxel local coordinates
/// </summary>
public List <Vector4D> FindPath(Vector3 start, Vector3 end)
{
float closestDistance = float.PositiveInfinity;
MyNavigationTriangle startTri = GetClosestNavigationTriangle(ref start, ref closestDistance);
if (startTri == null)
{
return(null);
}
closestDistance = float.PositiveInfinity;
MyNavigationTriangle endTri = GetClosestNavigationTriangle(ref end, ref closestDistance);
if (endTri == null)
{
return(null);
}
m_debugPos1 = Vector3.Transform(startTri.Position, m_voxelMap.PositionComp.WorldMatrix);
m_debugPos2 = Vector3.Transform(endTri.Position, m_voxelMap.PositionComp.WorldMatrix);
m_debugPos3 = Vector3.Transform(start, m_voxelMap.PositionComp.WorldMatrix);
m_debugPos4 = Vector3.Transform(end, m_voxelMap.PositionComp.WorldMatrix);
var path = FindRefinedPath(startTri, endTri, ref start, ref end);
return(path);
}
/// <summary>
/// All coords should be in the grid local coordinates
/// </summary>
public List <Vector4D> FindPath(Vector3 start, Vector3 end)
{
start /= m_grid.GridSize;
end /= m_grid.GridSize;
float closestDistSq = float.PositiveInfinity;
MyNavigationTriangle startTri = GetClosestNavigationTriangle(ref start, ref closestDistSq);
if (startTri == null)
{
return(null);
}
closestDistSq = float.PositiveInfinity;
MyNavigationTriangle endTri = GetClosestNavigationTriangle(ref end, ref closestDistSq);
if (endTri == null)
{
return(null);
}
var path = FindRefinedPath(startTri, endTri, ref start, ref end);
if (path != null)
{
for (int i = 0; i < path.Count; ++i)
{
var point = path[i];
point *= m_grid.GridSize;
path[i] = point;
}
}
return(path);
}
private MyNavigationTriangle GetClosestNavigationTriangle(ref Vector3 point, ref float closestDistanceSq)
{
MyNavigationTriangle triangle = null;
Vector3I vectori = Vector3I.Round((point + (this.m_voxelMap.PositionComp.GetPosition() - this.m_voxelMap.PositionLeftBottomCorner)) / this.m_cellSize);
for (int i = 0; i < 8; i++)
{
Vector3I position = (Vector3I)(vectori + m_cornerOffsets[i]);
if (this.m_processedCells.Contains(position))
{
ulong packedCellCoord = new MyCellCoord(0, position).PackId64();
MyIntervalList list = this.m_higherLevelHelper.TryGetTriangleList(packedCellCoord);
if (list != null)
{
MyIntervalList.Enumerator enumerator = list.GetEnumerator();
while (enumerator.MoveNext())
{
int current = enumerator.Current;
MyNavigationTriangle triangle2 = base.GetTriangle(current);
float num4 = Vector3.DistanceSquared(triangle2.Center, point);
if (num4 < closestDistanceSq)
{
closestDistanceSq = num4;
triangle = triangle2;
}
}
}
}
}
return(triangle);
}
private void RemoveTerrainTriangle(MyNavigationTriangle tri)
{
MyWingedEdgeMesh.FaceVertexEnumerator vertexEnumerator = tri.GetVertexEnumerator();
vertexEnumerator.MoveNext();
Vector3 current = vertexEnumerator.Current;
vertexEnumerator.MoveNext();
Vector3 vector2 = vertexEnumerator.Current;
vertexEnumerator.MoveNext();
Vector3 vector3 = vertexEnumerator.Current;
int edgeIndex = tri.GetEdgeIndex(0);
int num2 = tri.GetEdgeIndex(1);
int num3 = tri.GetEdgeIndex(2);
int num4 = edgeIndex;
if (!this.m_connectionHelper.TryRemoveOuterEdge(ref current, ref vector2, ref num4) && (base.Mesh.GetEdge(edgeIndex).OtherFace(tri.Index) != -1))
{
this.m_connectionHelper.AddOuterEdgeIndex(ref vector2, ref current, edgeIndex);
}
num4 = num2;
if (!this.m_connectionHelper.TryRemoveOuterEdge(ref vector2, ref vector3, ref num4) && (base.Mesh.GetEdge(num2).OtherFace(tri.Index) != -1))
{
this.m_connectionHelper.AddOuterEdgeIndex(ref vector3, ref vector2, num2);
}
num4 = num3;
if (!this.m_connectionHelper.TryRemoveOuterEdge(ref vector3, ref current, ref num4) && (base.Mesh.GetEdge(num3).OtherFace(tri.Index) != -1))
{
this.m_connectionHelper.AddOuterEdgeIndex(ref current, ref vector3, num3);
}
base.RemoveTriangle(tri);
}
private void GetEdgeVerticesSafe(MyNavigationTriangle triangle, int edgeIndex, out Vector3 left, out Vector3 right)
{
triangle.GetEdgeVertices(edgeIndex, out left, out right);
float d = (left - right).LengthSquared();
bool leftDangerous = triangle.IsEdgeVertexDangerous(edgeIndex, predVertex: true);
bool rightDangerous = triangle.IsEdgeVertexDangerous(edgeIndex, predVertex: false);
m_segmentDangerous |= leftDangerous | rightDangerous;
if (leftDangerous)
{
if (rightDangerous)
{
if (SAFE_DISTANCE2_SQ > d)
{
left = (left + right) * 0.5f;
right = left;
}
else
{
float t = SAFE_DISTANCE / (float)Math.Sqrt(d);
Vector3 newLeft = right * t + left * (1.0f - t);
right = left * t + right * (1.0f - t);
left = newLeft;
}
}
else
{
if (SAFE_DISTANCE_SQ > d)
{
left = right;
}
else
{
float t = SAFE_DISTANCE / (float)Math.Sqrt(d);
left = right * t + left * (1.0f - t);
}
}
}
else
{
if (rightDangerous)
{
if (SAFE_DISTANCE_SQ > d)
{
right = left;
}
else
{
float t = SAFE_DISTANCE / (float)Math.Sqrt(d);
right = left * t + right * (1.0f - t);
}
}
}
m_debugPointsLeft.Add(left);
m_debugPointsRight.Add(right);
}
public List <Vector4D> FindPath(Vector3 start, Vector3 end)
{
start /= this.m_grid.GridSize;
end /= this.m_grid.GridSize;
float positiveInfinity = float.PositiveInfinity;
MyNavigationTriangle closestNavigationTriangle = this.GetClosestNavigationTriangle(ref start, ref positiveInfinity);
if (closestNavigationTriangle == null)
{
return(null);
}
positiveInfinity = float.PositiveInfinity;
MyNavigationTriangle triangle2 = this.GetClosestNavigationTriangle(ref end, ref positiveInfinity);
if (triangle2 == null)
{
return(null);
}
List <Vector4D> list = base.FindRefinedPath(closestNavigationTriangle, triangle2, ref start, ref end);
if (list != null)
{
for (int i = 0; i < list.Count; i++)
{
Vector4D vectord = list[i] * this.m_grid.GridSize;
list[i] = vectord;
}
}
return(list);
}
public void RegisterTriangle(MyNavigationTriangle tri, ref Vector3I gridPos)
{
if (this.m_grid == null)
{
this.RegisterTriangleInternal(tri, ref gridPos);
}
}
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 AddComponentTriangle(MyNavigationTriangle triangle, Vector3 center)
{
MyIntervalList list = this.m_components[this.m_componentNum];
list.Add(triangle.Index);
float num2 = 1f / ((float)list.Count);
this.m_lastCellComponentCenters[this.m_componentNum] = (center * num2) + (this.m_lastCellComponentCenters[this.m_componentNum] * (1f - num2));
}
public bool Contains(MyNavigationPrimitive primitive)
{
if (!ReferenceEquals(primitive.Group, this.m_parent))
{
return(false);
}
MyNavigationTriangle triangle = primitive as MyNavigationTriangle;
return((triangle != null) ? (triangle.ComponentIndex == this.m_componentIndex) : false);
}
public void RegisterTriangle(MyNavigationTriangle tri, ref Vector3I gridPos)
{
Debug.Assert(m_grid == null, "Triangles can be registered in the grid navigation mesh from the outside only if it's a mesh for block navigation definition!");
if (m_grid != null)
{
return;
}
RegisterTriangleInternal(tri, ref gridPos);
}
private MyNavigationTriangle AddTriangleInternal(Vector3 a, Vector3 b, Vector3 c)
{
int num;
int num2;
int num3;
Vector3I pointB = Vector3I.Round(a * 256f);
Vector3I pointA = Vector3I.Round(b * 256f);
Vector3I vectori3 = Vector3I.Round(c * 256f);
Vector3 vector = pointB / 256f;
Vector3 vector2 = pointA / 256f;
Vector3 vector3 = vectori3 / 256f;
if (!this.m_connectionHelper.TryGetValue(new EdgeIndex(ref pointA, ref pointB), out num))
{
num = -1;
}
if (!this.m_connectionHelper.TryGetValue(new EdgeIndex(ref vectori3, ref pointA), out num2))
{
num2 = -1;
}
if (!this.m_connectionHelper.TryGetValue(new EdgeIndex(ref pointB, ref vectori3), out num3))
{
num3 = -1;
}
int num4 = num2;
int num5 = num3;
MyNavigationTriangle triangle = base.AddTriangle(ref vector, ref vector3, ref vector2, ref num3, ref num2, ref num);
if (num == -1)
{
this.m_connectionHelper.Add(new EdgeIndex(ref pointB, ref pointA), num);
}
else
{
this.m_connectionHelper.Remove(new EdgeIndex(ref pointA, ref pointB));
}
if (num4 == -1)
{
this.m_connectionHelper.Add(new EdgeIndex(ref pointA, ref vectori3), num2);
}
else
{
this.m_connectionHelper.Remove(new EdgeIndex(ref vectori3, ref pointA));
}
if (num5 == -1)
{
this.m_connectionHelper.Add(new EdgeIndex(ref vectori3, ref pointB), num3);
}
else
{
this.m_connectionHelper.Remove(new EdgeIndex(ref pointB, ref vectori3));
}
return(triangle);
}
bool IMyHighLevelComponent.Contains(MyNavigationPrimitive primitive)
{
MyNavigationTriangle tri = primitive as MyNavigationTriangle;
if (tri == null)
{
return(false);
}
return(tri.ComponentIndex == m_componentIndex);
}
private void RegisterTriangleInternal(MyNavigationTriangle tri, ref Vector3I gridPos)
{
List <int> list = null;
if (!this.m_smallTriangleRegistry.TryGetValue(gridPos, out list))
{
list = new List <int>();
this.m_smallTriangleRegistry.Add(gridPos, list);
}
list.Add(tri.Index);
tri.Registered = true;
}
private void RemoveTerrainTriangle(MyNavigationTriangle tri)
{
var vertices = tri.GetVertexEnumerator();
vertices.MoveNext();
Vector3 aPos = vertices.Current;
vertices.MoveNext();
Vector3 bPos = vertices.Current;
vertices.MoveNext();
Vector3 cPos = vertices.Current;
int edgeAB = tri.GetEdgeIndex(0);
int edgeBC = tri.GetEdgeIndex(1);
int edgeCA = tri.GetEdgeIndex(2);
int tmp;
ProfilerShort.Begin("Handling outer edges");
tmp = edgeAB;
if (!m_connectionHelper.TryRemoveOuterEdge(ref aPos, ref bPos, ref tmp))
{
var edge = Mesh.GetEdge(edgeAB);
if (edge.OtherFace(tri.Index) != -1)
{
m_connectionHelper.AddOuterEdgeIndex(ref bPos, ref aPos, edgeAB);
}
}
tmp = edgeBC;
if (!m_connectionHelper.TryRemoveOuterEdge(ref bPos, ref cPos, ref tmp))
{
var edge = Mesh.GetEdge(edgeBC);
if (edge.OtherFace(tri.Index) != -1)
{
m_connectionHelper.AddOuterEdgeIndex(ref cPos, ref bPos, edgeBC);
}
}
tmp = edgeCA;
if (!m_connectionHelper.TryRemoveOuterEdge(ref cPos, ref aPos, ref tmp))
{
var edge = Mesh.GetEdge(edgeCA);
if (edge.OtherFace(tri.Index) != -1)
{
m_connectionHelper.AddOuterEdgeIndex(ref aPos, ref cPos, edgeCA);
}
}
ProfilerShort.End();
ProfilerShort.Begin("Removing the tri");
RemoveTriangle(tri);
ProfilerShort.End();
}
protected MyNavigationTriangle AddTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C, ref int edgeAB, ref int edgeBC, ref int edgeCA)
{
MyNavigationTriangle userData = this.m_triPool.Allocate();
int num = ((0 + ((edgeAB == -1) ? 1 : 0)) + ((edgeBC == -1) ? 1 : 0)) + ((edgeCA == -1) ? 1 : 0);
int triangleIndex = -1;
if (num == 3)
{
triangleIndex = this.m_mesh.MakeNewTriangle(userData, ref A, ref B, ref C, out edgeAB, out edgeBC, out edgeCA);
}
else if (num == 2)
{
triangleIndex = (edgeAB == -1) ? ((edgeBC == -1) ? this.m_mesh.ExtrudeTriangleFromEdge(ref B, edgeCA, userData, out edgeAB, out edgeBC) : this.m_mesh.ExtrudeTriangleFromEdge(ref A, edgeBC, userData, out edgeCA, out edgeAB)) : this.m_mesh.ExtrudeTriangleFromEdge(ref C, edgeAB, userData, out edgeBC, out edgeCA);
}
else if (num == 1)
{
triangleIndex = (edgeAB != -1) ? ((edgeBC != -1) ? this.GetTriangleOneNewEdge(ref edgeCA, ref edgeAB, ref edgeBC, userData) : this.GetTriangleOneNewEdge(ref edgeBC, ref edgeCA, ref edgeAB, userData)) : this.GetTriangleOneNewEdge(ref edgeAB, ref edgeBC, ref edgeCA, userData);
}
else
{
MyWingedEdgeMesh.Edge other = this.m_mesh.GetEdge(edgeAB);
MyWingedEdgeMesh.Edge edge = this.m_mesh.GetEdge(edgeBC);
MyWingedEdgeMesh.Edge edge3 = this.m_mesh.GetEdge(edgeCA);
int sharedB = edge3.TryGetSharedVertex(ref other);
int sharedC = other.TryGetSharedVertex(ref edge);
int num5 = edge.TryGetSharedVertex(ref edge3);
int num6 = ((0 + ((sharedB == -1) ? 0 : 1)) + ((sharedC == -1) ? 0 : 1)) + ((num5 == -1) ? 0 : 1);
if (num6 == 3)
{
triangleIndex = this.m_mesh.MakeFace(userData, edgeAB);
}
else if (num6 == 2)
{
triangleIndex = (sharedB != -1) ? ((sharedC != -1) ? this.GetTriangleTwoSharedVertices(edgeCA, edgeAB, ref edgeBC, sharedB, sharedC, userData) : this.GetTriangleTwoSharedVertices(edgeBC, edgeCA, ref edgeAB, num5, sharedB, userData)) : this.GetTriangleTwoSharedVertices(edgeAB, edgeBC, ref edgeCA, sharedC, num5, userData);
}
else if (num6 == 1)
{
triangleIndex = (sharedB == -1) ? ((sharedC == -1) ? this.GetTriangleOneSharedVertex(edgeBC, edgeCA, ref edgeAB, num5, userData) : this.GetTriangleOneSharedVertex(edgeAB, edgeBC, ref edgeCA, sharedC, userData)) : this.GetTriangleOneSharedVertex(edgeCA, edgeAB, ref edgeBC, sharedB, userData);
}
else
{
int num7;
int num8;
triangleIndex = this.m_mesh.ExtrudeTriangleFromEdge(ref C, edgeAB, userData, out num7, out num8);
this.m_mesh.MergeEdges(num8, edgeCA);
this.m_mesh.MergeEdges(num7, edgeBC);
}
}
userData.Init(this, triangleIndex);
return(userData);
}
private void GetEdgeVerticesSafe(MyNavigationTriangle triangle, int edgeIndex, out Vector3 left, out Vector3 right)
{
triangle.GetEdgeVertices(edgeIndex, out left, out right);
float num = (left - right).LengthSquared();
bool flag = triangle.IsEdgeVertexDangerous(edgeIndex, true);
bool flag2 = triangle.IsEdgeVertexDangerous(edgeIndex, false);
this.m_segmentDangerous |= flag | flag2;
if (!flag)
{
if (flag2)
{
if (SAFE_DISTANCE_SQ > num)
{
right = left;
}
else
{
float num4 = SAFE_DISTANCE / ((float)Math.Sqrt((double)num));
right = (left * num4) + (right * (1f - num4));
}
}
}
else if (!flag2)
{
if (SAFE_DISTANCE_SQ > num)
{
left = right;
}
else
{
float num3 = SAFE_DISTANCE / ((float)Math.Sqrt((double)num));
left = (right * num3) + (left * (1f - num3));
}
}
else if (SAFE_DISTANCE2_SQ > num)
{
left = (left + right) * 0.5f;
right = left;
}
else
{
float num2 = SAFE_DISTANCE / ((float)Math.Sqrt((double)num));
Vector3 vector2 = (right * num2) + (left * (1f - num2));
right = (left * num2) + (right * (1f - num2));
left = vector2;
}
MyNavigationMesh.m_debugPointsLeft.Add(left);
MyNavigationMesh.m_debugPointsRight.Add(right);
}
private bool IsFaceTriangle(MyNavigationTriangle triangle, Vector3I cubePosition, Vector3I direction)
{
MyWingedEdgeMesh.FaceVertexEnumerator vertexEnumerator = triangle.GetVertexEnumerator();
vertexEnumerator.MoveNext();
vertexEnumerator.MoveNext();
vertexEnumerator.MoveNext();
cubePosition *= 0x100;
Vector3I vectori4 = (Vector3I)(cubePosition + (direction * 0x80));
Vector3I vectori = Vector3I.Round(vertexEnumerator.Current * 256f) - vectori4;
Vector3I vectori2 = Vector3I.Round(vertexEnumerator.Current * 256f) - vectori4;
Vector3I vectori3 = Vector3I.Round(vertexEnumerator.Current * 256f) - vectori4;
return(!((vectori * direction) != Vector3I.Zero) ? (!((vectori2 * direction) != Vector3I.Zero) ? (!((vectori3 * direction) != Vector3I.Zero) ? ((vectori.AbsMax() <= 0x80) && ((vectori2.AbsMax() <= 0x80) && (vectori3.AbsMax() <= 0x80))) : false) : false) : false);
}
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);
}
}
}
public void AddComponentTriangle(MyNavigationTriangle triangle, Vector3 center)
{
Debug.Assert(m_componentOpen, "Adding a triangle to a component in TriangleComponentMapping, when no component is open!");
int triIndex = triangle.Index;
MyIntervalList triList = m_components[m_componentNum];
triList.Add(triIndex);
float t = 1.0f / triList.Count;
m_lastCellComponentCenters[m_componentNum] = center * t + m_lastCellComponentCenters[m_componentNum] * (1.0f - t);
}
private int AddTriangle(int index)
{
if (!this.m_funnelConstructed)
{
this.ConstructFunnel(index);
}
else
{
Vector3 vector;
Vector3 vector2;
MyPath <MyNavigationPrimitive> .PathNode node = this.m_input[index];
MyNavigationTriangle vertex = node.Vertex as MyNavigationTriangle;
vertex.GetNavigationEdge(node.nextVertex);
this.GetEdgeVerticesSafe(vertex, node.nextVertex, out vector, out vector2);
PointTestResult result = this.TestPoint(vector);
PointTestResult result2 = this.TestPoint(vector2);
if (result == PointTestResult.INSIDE)
{
this.NarrowFunnel(vector, index, true);
}
if (result2 == PointTestResult.INSIDE)
{
this.NarrowFunnel(vector2, index, false);
}
if (result == PointTestResult.RIGHT)
{
this.m_apex = this.m_rightPoint;
this.m_funnelConstructed = false;
this.ConstructFunnel(this.m_rightIndex + 1);
return(this.m_rightIndex + 1);
}
if (result2 == PointTestResult.LEFT)
{
this.m_apex = this.m_leftPoint;
this.m_funnelConstructed = false;
this.ConstructFunnel(this.m_leftIndex + 1);
return(this.m_leftIndex + 1);
}
if ((result == PointTestResult.INSIDE) || (result2 == PointTestResult.INSIDE))
{
MyNavigationMesh.FunnelState item = new MyNavigationMesh.FunnelState {
Apex = this.m_apex,
Left = this.m_leftPoint,
Right = this.m_rightPoint
};
MyNavigationMesh.m_debugFunnel.Add(item);
}
}
return(index + 1);
}
private bool ProcessTriangleForHierarchy(MyNavigationTriangle triangle)
{
// The triangle parent can be wrong when we have multiple navmeshes connected via external edges
if (triangle.Parent != m_mesh)
{
return(false);
}
// Previously unvisited triangle will be assigned the current relative component index
if (triangle.ComponentIndex == -1)
{
m_navmeshComponents.AddComponentTriangle(triangle, triangle.Center);
m_tmpComponentTriangles.Add(triangle);
triangle.ComponentIndex = m_currentComponentMarker;
return(true);
}
else if (triangle.ComponentIndex == m_currentComponentMarker)
{
// We can safely ignore this triangle (it has already been processed);
return(true);
}
else
{
ulong cellIndex;
if (m_navmeshComponents.GetComponentCell(triangle.ComponentIndex, out cellIndex))
{
MyCellCoord cellCoord = new MyCellCoord();
cellCoord.SetUnpack(cellIndex);
Vector3I diff = cellCoord.CoordInLod - m_currentCell;
if (diff.RectangularLength() != 1)
{
// CH: TODO: Connection of components over cell edges or vertices. I currently silently ignore that...
return(false);
}
ConnectionInfo connection = new ConnectionInfo();
connection.Direction = Base6Directions.GetDirection(diff);
connection.ComponentIndex = triangle.ComponentIndex;
// Save connections to other components. There won't be so many, so we can keep them in a list instead of a HashSet
if (!m_currentCellConnections[m_currentComponentRel].Contains(connection))
{
m_currentCellConnections[m_currentComponentRel].Add(connection);
}
}
}
return(false);
}
private void FixCubeFace(ref Vector3I pos, ref Vector3I dir)
{
if (this.m_cubeSet.Contains(ref pos))
{
MySlimBlock cubeBlock = this.m_grid.GetCubeBlock(pos);
MyCompoundCubeBlock fatBlock = cubeBlock.FatBlock as MyCompoundCubeBlock;
if (fatBlock != null)
{
ListReader <MySlimBlock> blocks = fatBlock.GetBlocks();
MySlimBlock block3 = null;
foreach (MySlimBlock block4 in blocks)
{
if (block4.BlockDefinition.NavigationDefinition != null)
{
block3 = block4;
break;
}
}
if (block3 != null)
{
cubeBlock = block3;
}
}
if (cubeBlock.BlockDefinition.NavigationDefinition != null)
{
MatrixI xi2;
Vector3I vectori;
Vector3I vectori2;
List <int> list;
MatrixI matrix = new MatrixI(cubeBlock.Position, cubeBlock.Orientation.Forward, cubeBlock.Orientation.Up);
MatrixI.Invert(ref matrix, out xi2);
Vector3I.Transform(ref pos, ref xi2, out vectori);
Vector3I.TransformNormal(ref dir, ref xi2, out vectori2);
MyGridNavigationMesh mesh = cubeBlock.BlockDefinition.NavigationDefinition.Mesh;
if ((mesh != null) && mesh.m_smallTriangleRegistry.TryGetValue(vectori, out list))
{
foreach (int num in list)
{
MyNavigationTriangle triangle = mesh.GetTriangle(num);
if (this.IsFaceTriangle(triangle, vectori, vectori2))
{
this.CopyTriangle(triangle, vectori, ref matrix);
}
}
}
}
}
}
private void RegisterTriangleInternal(MyNavigationTriangle tri, ref Vector3I gridPos)
{
Debug.Assert(!tri.Registered); // So far, triangles can only be registered in one cube. This can change in the future with optimizations
List <int> list = null;
if (!m_smallTriangleRegistry.TryGetValue(gridPos, out list))
{
list = new List <int>();
m_smallTriangleRegistry.Add(gridPos, list);
}
list.Add(tri.Index);
tri.Registered = true;
}
public override MyNavigationPrimitive FindClosestPrimitive(Vector3D point, bool highLevel, ref double closestDistanceSq)
{
if (highLevel)
{
return(null);
}
Vector3 vector = (Vector3)(Vector3D.Transform(point, this.m_grid.PositionComp.WorldMatrixNormalizedInv) / this.m_grid.GridSize);
float closestDistSq = ((float)closestDistanceSq) / this.m_grid.GridSize;
MyNavigationTriangle closestNavigationTriangle = this.GetClosestNavigationTriangle(ref vector, ref closestDistSq);
if (closestNavigationTriangle != null)
{
closestDistanceSq = closestDistSq * this.m_grid.GridSize;
}
return(closestNavigationTriangle);
}
public MyHighLevelPrimitive GetHighLevelNavigationPrimitive(MyNavigationTriangle triangle)
{
Debug.Assert(triangle.Parent == this.m_mesh, "Finding cell of a navigation triangle in a wrong mesh!");
if (triangle.Parent != this.m_mesh)
{
return(null);
}
if (triangle.ComponentIndex != -1)
{
return(m_mesh.HighLevelGroup.GetPrimitive(triangle.ComponentIndex));
}
else
{
return(null);
}
}
private void RemoveAndAddTriangle(ref Vector3I positionA, ref Vector3I positionB, int registeredEdgeIndex)
{
MyNavigationTriangle edgeTriangle = base.GetEdgeTriangle(registeredEdgeIndex);
MyWingedEdgeMesh.FaceVertexEnumerator vertexEnumerator = edgeTriangle.GetVertexEnumerator();
vertexEnumerator.MoveNext();
Vector3 current = vertexEnumerator.Current;
vertexEnumerator.MoveNext();
vertexEnumerator.MoveNext();
Vector3I cube = this.FindTriangleCube(edgeTriangle.Index, ref positionA, ref positionB);
this.RemoveTriangle(edgeTriangle, cube);
MyNavigationTriangle tri = this.AddTriangleInternal(current, vertexEnumerator.Current, vertexEnumerator.Current);
this.RegisterTriangleInternal(tri, ref cube);
}
private bool ProcessTriangleForHierarchy(MyNavigationTriangle triangle)
{
if (ReferenceEquals(triangle.Parent, this.m_mesh))
{
ulong num;
if (m_tmpCellTriangles.Contains(triangle.Index))
{
this.m_components.AddComponentTriangle(triangle, triangle.Center);
this.m_tmpComponentTriangles.Add(triangle);
return(true);
}
if (this.m_components.TryGetComponentCell(triangle.ComponentIndex, out num) && !this.m_currentCellConnections[this.m_currentComponentRel].Contains(triangle.ComponentIndex))
{
this.m_currentCellConnections[this.m_currentComponentRel].Add(triangle.ComponentIndex);
}
}
return(false);
}
private bool ProcessTriangleForHierarchy(MyNavigationTriangle triangle)
{
// The triangle parent can be wrong when we have multiple navmeshes connected via external edges
if (triangle.Parent != m_mesh)
{
return false;
}
if (m_tmpCellTriangles.Contains(triangle.Index))
{
m_components.AddComponentTriangle(triangle, triangle.Center);
m_tmpComponentTriangles.Add(triangle);
return true;
}
else
{
ulong cellIndex;
// This test succeeds only if the triangle belongs to an unchanged component or to a component that was changed,
// but processed in a different cell already
if (m_components.TryGetComponentCell(triangle.ComponentIndex, out cellIndex))
{
// Save connections to other components. There won't be so many, so we can keep them in a list instead of a HashSet
if (!m_currentCellConnections[m_currentComponentRel].Contains(triangle.ComponentIndex))
{
m_currentCellConnections[m_currentComponentRel].Add(triangle.ComponentIndex);
}
}
}
return false;
}
public MyHighLevelPrimitive GetHighLevelNavigationPrimitive(MyNavigationTriangle triangle)
{
Debug.Assert(triangle != null, "Navigation triangle was null!");
if (triangle == null) return null;
Debug.Assert(triangle.Parent == this.m_mesh, "Finding cell of a navigation triangle in a wrong mesh!");
if (triangle.Parent != this.m_mesh)
{
return null;
}
if (triangle.ComponentIndex != -1)
{
return m_mesh.HighLevelGroup.GetPrimitive(triangle.ComponentIndex);
}
else
{
return null;
}
}
private void RemoveTriangle(MyNavigationTriangle triangle, Vector3I cube)
{
var e = triangle.GetVertexEnumerator();
e.MoveNext();
Vector3I positionA = Vector3I.Round(e.Current * 256);
e.MoveNext();
Vector3I positionC = Vector3I.Round(e.Current * 256); // This is not a mistake! C is swapped with B (see also: AddTriangle...)
e.MoveNext();
Vector3I positionB = Vector3I.Round(e.Current * 256);
// This should be AB, BC and CA, but after swapping it's AC, CB and BA
int triCA = triangle.GetEdgeIndex(0);
int triBC = triangle.GetEdgeIndex(1);
int triAB = triangle.GetEdgeIndex(2);
int edgeAB, edgeBC, edgeCA;
if (!m_connectionHelper.TryGetValue(new EdgeIndex(ref positionA, ref positionB), out edgeAB)) edgeAB = -1;
if (!m_connectionHelper.TryGetValue(new EdgeIndex(ref positionB, ref positionC), out edgeBC)) edgeBC = -1;
if (!m_connectionHelper.TryGetValue(new EdgeIndex(ref positionC, ref positionA), out edgeCA)) edgeCA = -1;
// Register those edges that were not registered in the connection helper.
// They are registered counter-clockwise, because we are in fact registering the other triangle across the edges
if (edgeAB != -1 && triAB == edgeAB)
m_connectionHelper.Remove(new EdgeIndex(ref positionA, ref positionB));
else
m_connectionHelper.Add(new EdgeIndex(positionB, positionA), triAB);
if (edgeBC != -1 && triBC == edgeBC)
m_connectionHelper.Remove(new EdgeIndex(ref positionB, ref positionC));
else
m_connectionHelper.Add(new EdgeIndex(positionC, positionB), triBC);
if (edgeCA != -1 && triCA == edgeCA)
m_connectionHelper.Remove(new EdgeIndex(ref positionC, ref positionA));
else
m_connectionHelper.Add(new EdgeIndex(positionA, positionC), triCA);
List<int> list = null;
m_smallTriangleRegistry.TryGetValue(cube, out list);
Debug.Assert(list != null, "Removed triangle was not registered in the registry!");
for (int i = 0; i < list.Count; ++i)
{
if (list[i] == triangle.Index)
{
list.RemoveAtFast(i);
break;
}
}
if (list.Count == 0)
m_smallTriangleRegistry.Remove(cube);
RemoveTriangle(triangle);
if (edgeAB != -1 && triAB != edgeAB)
RemoveAndAddTriangle(ref positionA, ref positionB, edgeAB);
if (edgeBC != -1 && triBC != edgeBC)
RemoveAndAddTriangle(ref positionB, ref positionC, edgeBC);
if (edgeCA != -1 && triCA != edgeCA)
RemoveAndAddTriangle(ref positionC, ref positionA, edgeCA);
}
private bool IsFaceTriangle(MyNavigationTriangle triangle, Vector3I cubePosition, Vector3I direction)
{
var e = triangle.GetVertexEnumerator();
// Quantize the point positions to 1/256ths inside each cube (but multiplied by 256 :-) )
e.MoveNext();
Vector3I positionA = Vector3I.Round(e.Current * 256);
e.MoveNext();
Vector3I positionB = Vector3I.Round(e.Current * 256);
e.MoveNext();
Vector3I positionC = Vector3I.Round(e.Current * 256);
cubePosition = cubePosition * 256;
// Change the point coordinates to be relative to the face center
Vector3I faceCenter = cubePosition + direction * 128;
positionA -= faceCenter;
positionB -= faceCenter;
positionC -= faceCenter;
// Discard all triangles whose points are not on the plane of the face
if (positionA * direction != Vector3I.Zero) return false;
if (positionB * direction != Vector3I.Zero) return false;
if (positionC * direction != Vector3I.Zero) return false;
// Discard all triangles that are not contained inside the face's square
return (positionA.AbsMax() <= 128 && positionB.AbsMax() <= 128 && positionC.AbsMax() <= 128);
}
private void CopyTriangle(MyNavigationTriangle otherTri, Vector3I triPosition, ref MatrixI transform)
{
Vector3 newA, newB, newC;
otherTri.GetTransformed(ref transform, out newA, out newB, out newC);
if (MyFakes.NAVMESH_PRESUMES_DOWNWARD_GRAVITY)
{
Vector3 n = Vector3.Cross(newC - newA, newB - newA);
n.Normalize();
if (Vector3.Dot(n, Base6Directions.GetVector(Base6Directions.Direction.Up)) < 0.7f) return; // Slightly lower than sqrt(2)/2 = 45 deg
}
Vector3I.Transform(ref triPosition, ref transform, out triPosition);
// This is not an error - we need to swap C and B from the navmesh,
// because they will be swapped again when the triangle is added
var tri = AddTriangleInternal(newA, newC, newB);
RegisterTriangleInternal(tri, ref triPosition);
}
private void RemoveTerrainTriangle(MyNavigationTriangle tri)
{
var vertices = tri.GetVertexEnumerator();
vertices.MoveNext();
Vector3 aPos = vertices.Current;
vertices.MoveNext();
Vector3 bPos = vertices.Current;
vertices.MoveNext();
Vector3 cPos = vertices.Current;
int edgeAB = tri.GetEdgeIndex(0);
int edgeBC = tri.GetEdgeIndex(1);
int edgeCA = tri.GetEdgeIndex(2);
int tmp;
ProfilerShort.Begin("Handling outer edges");
tmp = edgeAB;
if (!m_connectionHelper.TryRemoveOuterEdge(ref aPos, ref bPos, ref tmp))
{
var edge = Mesh.GetEdge(edgeAB);
if (edge.OtherFace(tri.Index) != -1)
m_connectionHelper.AddOuterEdgeIndex(ref bPos, ref aPos, edgeAB);
}
tmp = edgeBC;
if (!m_connectionHelper.TryRemoveOuterEdge(ref bPos, ref cPos, ref tmp))
{
var edge = Mesh.GetEdge(edgeBC);
if (edge.OtherFace(tri.Index) != -1)
m_connectionHelper.AddOuterEdgeIndex(ref cPos, ref bPos, edgeBC);
}
tmp = edgeCA;
if (!m_connectionHelper.TryRemoveOuterEdge(ref cPos, ref aPos, ref tmp))
{
var edge = Mesh.GetEdge(edgeCA);
if (edge.OtherFace(tri.Index) != -1)
m_connectionHelper.AddOuterEdgeIndex(ref aPos, ref cPos, edgeCA);
}
ProfilerShort.End();
ProfilerShort.Begin("Removing the tri");
RemoveTriangle(tri);
ProfilerShort.End();
}
private void RegisterTriangleInternal(MyNavigationTriangle tri, ref Vector3I gridPos)
{
Debug.Assert(!tri.Registered); // So far, triangles can only be registered in one cube. This can change in the future with optimizations
List<int> list = null;
if (!m_smallTriangleRegistry.TryGetValue(gridPos, out list))
{
list = new List<int>();
m_smallTriangleRegistry.Add(gridPos, list);
}
list.Add(tri.Index);
tri.Registered = true;
}
private bool ProcessTriangleForHierarchy(MyNavigationTriangle triangle)
{
// The triangle parent can be wrong when we have multiple navmeshes connected via external edges
if (triangle.Parent != m_mesh)
{
return false;
}
if (triangle.ComponentIndex == -1)
{
m_navmeshComponents.AddComponentTriangle(triangle, triangle.Center);
triangle.ComponentIndex = m_navmeshComponents.OpenComponentIndex;
return true;
}
else if (triangle.ComponentIndex == m_navmeshComponents.OpenComponentIndex)
{
return true;
}
else
{
ulong cellIndex;
if (m_navmeshComponents.GetComponentCell(triangle.ComponentIndex, out cellIndex))
{
MyCellCoord cellCoord = new MyCellCoord();
cellCoord.SetUnpack(cellIndex);
Vector3I diff = cellCoord.CoordInLod - m_currentCell;
if (diff.RectangularLength() != 1)
{
// CH: TODO: Connection of components over cell edges or vertices. I currently silently ignore that...
return false;
}
ConnectionInfo connection = new ConnectionInfo();
connection.Direction = Base6Directions.GetDirection(diff);
connection.ComponentIndex = triangle.ComponentIndex;
// Save connections to other components. There won't be so many, so we can keep them in a list instead of a HashSet
if (!m_currentCellConnections[m_currentComponentRel].Contains(connection))
{
m_currentCellConnections[m_currentComponentRel].Add(connection);
}
}
}
return false;
}
// 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();
}
public void AddComponentTriangle(MyNavigationTriangle triangle, Vector3 center)
{
Debug.Assert(m_componentOpen, "Adding a triangle to a component in TriangleComponentMapping, when no component is open!");
int triIndex = triangle.Index;
MyIntervalList triList = m_components[m_componentNum];
triList.Add(triIndex);
float t = 1.0f / triList.Count;
m_lastCellComponentCenters[m_componentNum] = center * t + m_lastCellComponentCenters[m_componentNum] * (1.0f - t);
}
public void RegisterTriangle(MyNavigationTriangle tri, ref Vector3I gridPos)
{
Debug.Assert(m_grid == null, "Triangles can be registered in the grid navigation mesh from the outside only if it's a mesh for block navigation definition!");
if (m_grid != null)
{
return;
}
RegisterTriangleInternal(tri, ref gridPos);
}
