private float GetDensity(ref Vector3D voxelCoord, int iterations)
{
// ReSharper disable once ImpureMethodCallOnReadonlyValueField
if (Box.Contains(voxelCoord) == ContainmentType.Disjoint)
{
return(0f);
}
var inflatedBox = new BoundingBoxD(voxelCoord - 0.5f, voxelCoord + 0.5f);
var queryBox = new BoundingBoxD(inflatedBox.Min - 0.1f, inflatedBox.Max + 0.1f + QueryDir * 1000);
{
var hits = 0;
foreach (var k in _tris)
{
if (!inflatedBox.IntersectsTriangle(k.Origin, k.Origin + k.Edge1, k.Origin + k.Edge2))
{
continue;
}
hits++;
break;
}
if (hits == 0)
{
return(IsInside(voxelCoord) ? 1 : 0);
}
}
List <int> tmp;
if (!_stackBorrow.TryPop(out tmp))
{
tmp = new List <int>();
}
else
{
tmp.Clear();
}
QueryTriangles(queryBox, tmp);
var max = new Vector3I(iterations, iterations, iterations);
var hit = 0;
for (var itr = new Vector3I_RangeIterator(ref Vector3I.Zero, ref max); itr.IsValid(); itr.MoveNext())
{
var sample = inflatedBox.Min + inflatedBox.Extents * itr.Current / iterations;
if (IsInsideHelper(sample, tmp))
{
hit++;
}
}
_stackBorrow.Push(tmp);
return(hit / (float)((iterations + 1) * (iterations + 1) * (iterations + 1)));
}
internal void QueryTriangles(BoundingBoxD queryBox, List <int> indices)
{
for (var index = 0; index < _tris.Length; index++)
{
var k = _tris[index];
if (queryBox.IntersectsTriangle(k.Origin, k.Origin + k.Edge1, k.Origin + k.Edge2))
{
indices.Add(index);
}
}
}
public static void IntersectSmallBox(int[] closestFace, Vector3D[] physicsVerts, BoundingBoxD bWorldAabb, List <Vector3D> intersections)
{
for (int i = 0; i < closestFace.Length; i += 3)
{
var v0 = physicsVerts[closestFace[i]];
var v1 = physicsVerts[closestFace[i + 1]];
var v2 = physicsVerts[closestFace[i + 2]];
var test1 = bWorldAabb.IntersectsTriangle(v0, v1, v2);
if (!test1)
{
continue;
}
intersections.Add(v0);
intersections.Add(v1);
intersections.Add(v2);
}
}
internal bool IntersectsSurface(ref Vector3D voxelCoord)
{
if (Box.Contains(voxelCoord) == ContainmentType.Disjoint)
{
return(false);
}
var inflatedBox = new BoundingBoxD(voxelCoord - 0.5f, voxelCoord + 0.5f);
foreach (var k in _tris)
{
if (inflatedBox.IntersectsTriangle(k.Origin, k.Origin + k.Edge1, k.Origin + k.Edge2))
{
return(true);
}
}
return(false);
}
public void TestVoxelNavmeshTriangle(ref Vector3D a, ref Vector3D b, ref Vector3D c, List<MyCubeGrid> gridsToTest, List<MyGridPathfinding.CubeId> linkCandidatesOutput, out bool intersecting)
{
ProfilerShort.Begin("TestVoxelNavmeshTriangle");
ProfilerShort.Begin("Triangle-obstacle tests");
Vector3D s = (a + b + c) / 3.0;
if (m_obstacles.IsInObstacle(s))
{
intersecting = true;
ProfilerShort.End();
ProfilerShort.End();
return;
}
ProfilerShort.End();
BoundingBoxD triBB;
Vector3D aLocal, bLocal, cLocal, gLocal;
Vector3D g = Vector3D.Zero;
if (MyPerGameSettings.NavmeshPresumesDownwardGravity)
{
g = Vector3.Down * 2.0f;
}
m_tmpLinkCandidates.Clear();
intersecting = false;
foreach (var grid in gridsToTest)
{
MatrixD mat = grid.PositionComp.WorldMatrixNormalizedInv;
Vector3D.Transform(ref a, ref mat, out aLocal);
Vector3D.Transform(ref b, ref mat, out bLocal);
Vector3D.Transform(ref c, ref mat, out cLocal);
Vector3D.TransformNormal(ref g, ref mat, out gLocal);
triBB = new BoundingBoxD(Vector3D.MaxValue, Vector3D.MinValue);
triBB.Include(ref aLocal, ref bLocal, ref cLocal);
Vector3I min = grid.LocalToGridInteger(triBB.Min);
Vector3I max = grid.LocalToGridInteger(triBB.Max);
Vector3I pos = min - Vector3I.One;
Vector3I max2 = max + Vector3I.One;
for (var it = new Vector3I_RangeIterator(ref pos, ref max2); it.IsValid(); it.GetNext(out pos))
{
if (grid.GetCubeBlock(pos) != null)
{
Vector3 largeMin = (pos - Vector3.One) * grid.GridSize;
Vector3 largeMax = (pos + Vector3.One) * grid.GridSize;
Vector3 smallMin = (pos - Vector3.Half) * grid.GridSize;
Vector3 smallMax = (pos + Vector3.Half) * grid.GridSize;
BoundingBoxD largeBb = new BoundingBoxD(largeMin, largeMax);
BoundingBoxD bb = new BoundingBoxD(smallMin, smallMax);
largeBb.Include(largeMin + gLocal);
largeBb.Include(largeMax + gLocal);
bb.Include(smallMin + gLocal);
bb.Include(smallMax + gLocal);
ProfilerShort.Begin("Triangle intersection tests");
if (largeBb.IntersectsTriangle(ref aLocal, ref bLocal, ref cLocal))
{
if (bb.IntersectsTriangle(ref aLocal, ref bLocal, ref cLocal))
{
intersecting = true;
ProfilerShort.End();
break;
}
else
{
int dx = Math.Min(Math.Abs(min.X - pos.X), Math.Abs(max.X - pos.X));
int dy = Math.Min(Math.Abs(min.Y - pos.Y), Math.Abs(max.Y - pos.Y));
int dz = Math.Min(Math.Abs(min.Z - pos.Z), Math.Abs(max.Z - pos.Z));
if ((dx + dy + dz) < 3)
m_tmpLinkCandidates.Add(new MyGridPathfinding.CubeId() { Grid = grid, Coords = pos });
}
}
ProfilerShort.End();
}
}
if (intersecting) break;
}
if (!intersecting)
{
for (int i = 0; i < m_tmpLinkCandidates.Count; ++i)
{
linkCandidatesOutput.Add(m_tmpLinkCandidates[i]);
}
}
m_tmpLinkCandidates.Clear();
ProfilerShort.End();
}
public void TestVoxelNavmeshTriangle(ref Vector3D a, ref Vector3D b, ref Vector3D c, List <MyCubeGrid> gridsToTest, List <MyGridPathfinding.CubeId> linkCandidatesOutput, out bool intersecting)
{
Vector3D point = ((a + b) + c) / 3.0;
if (this.m_obstacles.IsInObstacle(point))
{
intersecting = true;
return;
}
Vector3D zero = Vector3D.Zero;
if (MyPerGameSettings.NavmeshPresumesDownwardGravity)
{
zero = Vector3.Down * 2f;
}
m_tmpLinkCandidates.Clear();
intersecting = false;
using (List <MyCubeGrid> .Enumerator enumerator = gridsToTest.GetEnumerator())
{
goto TR_0016;
TR_0008:
if (intersecting)
{
goto TR_0006;
}
TR_0016:
while (true)
{
if (enumerator.MoveNext())
{
Vector3D vectord2;
Vector3D vectord3;
Vector3D vectord4;
Vector3D vectord5;
MyCubeGrid current = enumerator.Current;
MatrixD worldMatrixNormalizedInv = current.PositionComp.WorldMatrixNormalizedInv;
Vector3D.Transform(ref a, ref worldMatrixNormalizedInv, out vectord2);
Vector3D.Transform(ref b, ref worldMatrixNormalizedInv, out vectord3);
Vector3D.Transform(ref c, ref worldMatrixNormalizedInv, out vectord4);
Vector3D.TransformNormal(ref zero, ref worldMatrixNormalizedInv, out vectord5);
BoundingBoxD xd = new BoundingBoxD(Vector3D.MaxValue, Vector3D.MinValue);
xd.Include(ref vectord2, ref vectord3, ref vectord4);
Vector3I vectori = current.LocalToGridInteger((Vector3)xd.Min);
Vector3I vectori2 = current.LocalToGridInteger((Vector3)xd.Max);
Vector3I start = vectori - Vector3I.One;
Vector3I end = (Vector3I)(vectori2 + Vector3I.One);
Vector3I_RangeIterator iterator = new Vector3I_RangeIterator(ref start, ref end);
while (iterator.IsValid())
{
if (current.GetCubeBlock(start) != null)
{
Vector3 min = (Vector3)((start - Vector3.One) * current.GridSize);
Vector3 max = (start + Vector3.One) * current.GridSize;
Vector3 vector3 = (Vector3)((start - Vector3.Half) * current.GridSize);
Vector3 vector4 = (start + Vector3.Half) * current.GridSize;
BoundingBoxD xd3 = new BoundingBoxD(min, max);
BoundingBoxD xd4 = new BoundingBoxD(vector3, vector4);
xd3.Include(min + vectord5);
xd3.Include(max + vectord5);
xd4.Include(vector3 + vectord5);
xd4.Include(vector4 + vectord5);
if (xd3.IntersectsTriangle(ref vectord2, ref vectord3, ref vectord4))
{
if (xd4.IntersectsTriangle(ref vectord2, ref vectord3, ref vectord4))
{
intersecting = true;
break;
}
int num3 = Math.Min(Math.Abs((int)(vectori.Z - start.Z)), Math.Abs((int)(vectori2.Z - start.Z)));
if (((Math.Min(Math.Abs((int)(vectori.X - start.X)), Math.Abs((int)(vectori2.X - start.X))) + Math.Min(Math.Abs((int)(vectori.Y - start.Y)), Math.Abs((int)(vectori2.Y - start.Y)))) + num3) < 3)
{
MyGridPathfinding.CubeId item = new MyGridPathfinding.CubeId {
Grid = current,
Coords = start
};
m_tmpLinkCandidates.Add(item);
}
}
}
iterator.GetNext(out start);
}
}
else
{
goto TR_0006;
}
break;
}
goto TR_0008;
}
TR_0006:
if (!intersecting)
{
for (int i = 0; i < m_tmpLinkCandidates.Count; i++)
{
linkCandidatesOutput.Add(m_tmpLinkCandidates[i]);
}
}
m_tmpLinkCandidates.Clear();
}
public void TestVoxelNavmeshTriangle(ref Vector3D a, ref Vector3D b, ref Vector3D c, List <MyCubeGrid> gridsToTest, List <MyGridPathfinding.CubeId> linkCandidatesOutput, out bool intersecting)
{
ProfilerShort.Begin("TestVoxelNavmeshTriangle");
ProfilerShort.Begin("Triangle-obstacle tests");
Vector3D s = (a + b + c) / 3.0;
if (m_obstacles.IsInObstacle(s))
{
intersecting = true;
ProfilerShort.End();
ProfilerShort.End();
return;
}
ProfilerShort.End();
BoundingBoxD triBB;
Vector3D aLocal, bLocal, cLocal, gLocal;
Vector3D g = Vector3D.Zero;
if (MyPerGameSettings.NavmeshPresumesDownwardGravity)
{
g = Vector3.Down * 2.0f;
}
m_tmpLinkCandidates.Clear();
intersecting = false;
foreach (var grid in gridsToTest)
{
MatrixD mat = grid.PositionComp.WorldMatrixNormalizedInv;
Vector3D.Transform(ref a, ref mat, out aLocal);
Vector3D.Transform(ref b, ref mat, out bLocal);
Vector3D.Transform(ref c, ref mat, out cLocal);
Vector3D.TransformNormal(ref g, ref mat, out gLocal);
triBB = new BoundingBoxD(Vector3D.MaxValue, Vector3D.MinValue);
triBB.Include(ref aLocal, ref bLocal, ref cLocal);
Vector3I min = grid.LocalToGridInteger(triBB.Min);
Vector3I max = grid.LocalToGridInteger(triBB.Max);
Vector3I pos = min - Vector3I.One;
Vector3I max2 = max + Vector3I.One;
for (var it = new Vector3I_RangeIterator(ref pos, ref max2); it.IsValid(); it.GetNext(out pos))
{
if (grid.GetCubeBlock(pos) != null)
{
Vector3 largeMin = (pos - Vector3.One) * grid.GridSize;
Vector3 largeMax = (pos + Vector3.One) * grid.GridSize;
Vector3 smallMin = (pos - Vector3.Half) * grid.GridSize;
Vector3 smallMax = (pos + Vector3.Half) * grid.GridSize;
BoundingBoxD largeBb = new BoundingBoxD(largeMin, largeMax);
BoundingBoxD bb = new BoundingBoxD(smallMin, smallMax);
largeBb.Include(largeMin + gLocal);
largeBb.Include(largeMax + gLocal);
bb.Include(smallMin + gLocal);
bb.Include(smallMax + gLocal);
ProfilerShort.Begin("Triangle intersection tests");
if (largeBb.IntersectsTriangle(ref aLocal, ref bLocal, ref cLocal))
{
if (bb.IntersectsTriangle(ref aLocal, ref bLocal, ref cLocal))
{
intersecting = true;
ProfilerShort.End();
break;
}
else
{
int dx = Math.Min(Math.Abs(min.X - pos.X), Math.Abs(max.X - pos.X));
int dy = Math.Min(Math.Abs(min.Y - pos.Y), Math.Abs(max.Y - pos.Y));
int dz = Math.Min(Math.Abs(min.Z - pos.Z), Math.Abs(max.Z - pos.Z));
if ((dx + dy + dz) < 3)
{
m_tmpLinkCandidates.Add(new MyGridPathfinding.CubeId()
{
Grid = grid, Coords = pos
});
}
}
}
ProfilerShort.End();
}
}
if (intersecting)
{
break;
}
}
if (!intersecting)
{
for (int i = 0; i < m_tmpLinkCandidates.Count; ++i)
{
linkCandidatesOutput.Add(m_tmpLinkCandidates[i]);
}
}
m_tmpLinkCandidates.Clear();
ProfilerShort.End();
}
