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