private static void MaintainEdge(int a, int b, ref QuickList <int> edges)
{
bool contained = false;
int index = 0;
for (int k = 0; k < edges.Count; k += 2)
{
if ((edges[k] == a && edges[k + 1] == b) || (edges[k] == b && edges[k + 1] == a))
{
contained = true;
index = k;
}
}
//If it isn't present, add it to the edge list.
if (!contained)
{
edges.Add(a);
edges.Add(b);
}
else
{
//If it is present, that means both edge-connected triangles were deleted now, so get rid of it.
edges.FastRemoveAt(index + 1);
edges.FastRemoveAt(index);
}
}
/// <summary>
/// Removes redundant points. Two points are redundant if they occupy the same hash grid cell.
/// </summary>
/// <param name="points">List of points to prune.</param>
/// <param name="cellSize">Size of cells to determine redundancy.</param>
public static void RemoveRedundantPoints(ref QuickList<Vector3> points, double cellSize)
{
var set = new QuickSet<Int3>(BufferPools<Int3>.Locking, BufferPools<int>.Locking, BufferPool.GetPoolIndex(points.Count));
for (int i = points.Count - 1; i >= 0; --i)
{
var element = points.Elements[i];
var cell = new Int3
{
X = (int)Math.Floor(element.X / cellSize),
Y = (int)Math.Floor(element.Y / cellSize),
Z = (int)Math.Floor(element.Z / cellSize)
};
if (set.Contains(cell))
{
points.FastRemoveAt(i);
}
else
{
set.Add(cell);
//TODO: Consider adding adjacent cells to guarantee that a point on the border between two cells will still detect the presence
//of a point on the opposite side of that border.
}
}
set.Dispose();
}
/// <summary>
/// Removes redundant points. Two points are redundant if they occupy the same hash grid cell.
/// </summary>
/// <param name="points">List of points to prune.</param>
/// <param name="cellSize">Size of cells to determine redundancy.</param>
public static void RemoveRedundantPoints(ref QuickList <Vector3> points, double cellSize)
{
var set = new QuickSet <Int3>(BufferPools <Int3> .Locking, BufferPools <int> .Locking, BufferPool.GetPoolIndex(points.Count));
for (int i = points.Count - 1; i >= 0; --i)
{
var element = points.Elements[i];
var cell = new Int3
{
X = (int)Math.Floor(element.X / cellSize),
Y = (int)Math.Floor(element.Y / cellSize),
Z = (int)Math.Floor(element.Z / cellSize)
};
if (set.Contains(cell))
{
points.FastRemoveAt(i);
}
else
{
set.Add(cell);
//TODO: Consider adding adjacent cells to guarantee that a point on the border between two cells will still detect the presence
//of a point on the opposite side of that border.
}
}
set.Dispose();
}
private static void RemoveInsidePoints(ref QuickList <Vector3> points, ref QuickList <int> triangleIndices, ref QuickList <int> outsidePoints)
{
var insidePoints = new QuickList <int>(BufferPools <int> .Locking);
//We're going to remove points from this list as we go to prune it down to the truly inner points.
insidePoints.AddRange(outsidePoints);
outsidePoints.Clear();
for (int i = 0; i < triangleIndices.Count && insidePoints.Count > 0; i += 3)
{
//Compute the triangle's plane in point-normal representation to test other points against.
Vector3 normal;
FindNormal(ref triangleIndices, ref points, i, out normal);
Vector3 p = points.Elements[triangleIndices.Elements[i]];
for (int j = insidePoints.Count - 1; j >= 0; --j)
{
//Offset from the triangle to the current point, tested against the normal, determines if the current point is visible
//from the triangle face.
Vector3 offset = points.Elements[insidePoints.Elements[j]] - p;
float dot = Vector3.Dot(offset, normal);
//If it's visible, then it's outside!
if (dot > 0)
{
//This point is known to be on the outside; put it on the outside!
outsidePoints.Add(insidePoints.Elements[j]);
insidePoints.FastRemoveAt(j);
}
}
}
insidePoints.Dispose();
}
public static void TestChurnStability()
{
var pool = new BufferPool();
var allocator = new Allocator(2048, pool);
var random = new Random(5);
ulong idCounter = 0;
var allocatedIds = new QuickList <ulong>(8, pool);
var unallocatedIds = new QuickList <ulong>(8, pool);
for (int i = 0; i < 512; ++i)
{
long start;
var id = idCounter++;
//allocator.ValidatePointers();
if (allocator.Allocate(id, 1 + random.Next(5), out start))
{
allocatedIds.Add(id, pool);
}
else
{
unallocatedIds.Add(id, pool);
}
//allocator.ValidatePointers();
}
for (int timestepIndex = 0; timestepIndex < 100000; ++timestepIndex)
{
//First add and remove a bunch randomly.
for (int i = random.Next(Math.Min(allocatedIds.Count, 15)); i >= 0; --i)
{
var indexToRemove = random.Next(allocatedIds.Count);
//allocator.ValidatePointers();
var deallocated = allocator.Deallocate(allocatedIds[indexToRemove]);
Debug.Assert(deallocated);
//allocator.ValidatePointers();
unallocatedIds.Add(allocatedIds[indexToRemove], pool);
allocatedIds.FastRemoveAt(indexToRemove);
}
for (int i = random.Next(Math.Min(unallocatedIds.Count, 15)); i >= 0; --i)
{
var indexToAllocate = random.Next(unallocatedIds.Count);
//allocator.ValidatePointers();
if (allocator.Allocate(unallocatedIds[indexToAllocate], random.Next(3), out long start))
{
//allocator.ValidatePointers();
allocatedIds.Add(unallocatedIds[indexToAllocate], pool);
unallocatedIds.FastRemoveAt(indexToAllocate);
}
//allocator.ValidatePointers();
}
//Check to ensure that everything's still coherent.
for (int i = 0; i < allocatedIds.Count; ++i)
{
Debug.Assert(allocator.Contains(allocatedIds[i]));
}
for (int i = 0; i < unallocatedIds.Count; ++i)
{
Debug.Assert(!allocator.Contains(unallocatedIds[i]));
}
}
//Wind it down.
for (int i = 0; i < allocatedIds.Count; ++i)
{
var deallocated = allocator.Deallocate(allocatedIds[i]);
Debug.Assert(deallocated);
}
//Confirm cleanup.
for (int i = 0; i < allocatedIds.Count; ++i)
{
Debug.Assert(!allocator.Contains(allocatedIds[i]));
}
for (int i = 0; i < unallocatedIds.Count; ++i)
{
Debug.Assert(!allocator.Contains(unallocatedIds[i]));
}
pool.Clear();
}
public static void TestChurnStability()
{
var allocator = new Allocator(2048);
var random = new Random(5);
ulong idCounter = 0;
var allocatedIds = new QuickList<ulong>(BufferPools<ulong>.Locking);
var unallocatedIds = new QuickList<ulong>(BufferPools<ulong>.Locking);
for (int i = 0; i < 512; ++i)
{
long start;
var id = idCounter++;
//allocator.ValidatePointers();
if (allocator.Allocate(id, 1 + random.Next(5), out start))
{
allocatedIds.Add(id);
}
else
{
unallocatedIds.Add(id);
}
//allocator.ValidatePointers();
}
for (int timestepIndex = 0; timestepIndex < 100000; ++timestepIndex)
{
//First add and remove a bunch randomly.
for (int i = random.Next(Math.Min(allocatedIds.Count, 15)); i >= 0; --i)
{
var indexToRemove = random.Next(allocatedIds.Count);
//allocator.ValidatePointers();
Assert.IsTrue(allocator.Deallocate(allocatedIds.Elements[indexToRemove]));
//allocator.ValidatePointers();
unallocatedIds.Add(allocatedIds.Elements[indexToRemove]);
allocatedIds.FastRemoveAt(indexToRemove);
}
for (int i = random.Next(Math.Min(unallocatedIds.Count, 15)); i >= 0; --i)
{
var indexToAllocate = random.Next(unallocatedIds.Count);
long start;
//allocator.ValidatePointers();
if (allocator.Allocate(unallocatedIds.Elements[indexToAllocate], random.Next(3), out start))
{
//allocator.ValidatePointers();
allocatedIds.Add(unallocatedIds.Elements[indexToAllocate]);
unallocatedIds.FastRemoveAt(indexToAllocate);
}
//allocator.ValidatePointers();
}
//Check to ensure that everything's still coherent.
for (int i = 0; i < allocatedIds.Count; ++i)
{
Assert.IsTrue(allocator.Contains(allocatedIds.Elements[i]));
}
for (int i = 0; i < unallocatedIds.Count; ++i)
{
Assert.IsFalse(allocator.Contains(unallocatedIds.Elements[i]));
}
}
//Wind it down.
for (int i = 0; i < allocatedIds.Count; ++i)
{
Assert.IsTrue(allocator.Deallocate(allocatedIds.Elements[i]));
}
//Confirm cleanup.
for (int i = 0; i < allocatedIds.Count; ++i)
{
Assert.IsFalse(allocator.Contains(allocatedIds.Elements[i]));
}
for (int i = 0; i < unallocatedIds.Count; ++i)
{
Assert.IsFalse(allocator.Contains(unallocatedIds.Elements[i]));
}
}
public static void Test()
{
var pool = new BufferPool();
var tree = new Tree(pool, 128);
const int leafCountAlongXAxis = 11;
const int leafCountAlongYAxis = 13;
const int leafCountAlongZAxis = 15;
var leafCount = leafCountAlongXAxis * leafCountAlongYAxis * leafCountAlongZAxis;
pool.Take <BoundingBox>(leafCount, out var leafBounds);
const float boundsSpan = 2;
const float spanRange = 2;
const float boundsSpacing = 3;
var random = new Random(5);
for (int i = 0; i < leafCountAlongXAxis; ++i)
{
for (int j = 0; j < leafCountAlongYAxis; ++j)
{
for (int k = 0; k < leafCountAlongZAxis; ++k)
{
var index = leafCountAlongXAxis * leafCountAlongYAxis * k + leafCountAlongXAxis * j + i;
leafBounds[index].Min = new Vector3(i, j, k) * boundsSpacing;
leafBounds[index].Max = leafBounds[index].Min + new Vector3(boundsSpan) +
spanRange * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
}
}
}
var prebuiltCount = Math.Max(leafCount / 2, 1);
tree.SweepBuild(pool, leafBounds.Slice(prebuiltCount));
tree.Validate();
for (int i = prebuiltCount; i < leafCount; ++i)
{
tree.Add(ref leafBounds[i], pool);
}
tree.Validate();
pool.TakeAtLeast <int>(leafCount, out var handleToLeafIndex);
pool.TakeAtLeast <int>(leafCount, out var leafIndexToHandle);
for (int i = 0; i < leafCount; ++i)
{
handleToLeafIndex[i] = i;
leafIndexToHandle[i] = i;
}
const int iterations = 100000;
const int maximumChangesPerIteration = 20;
var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount);
var refineContext = new Tree.RefitAndRefineMultithreadedContext();
var selfTestContext = new Tree.MultithreadedSelfTest <OverlapHandler>(pool);
var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount];
Action <int> pairTestAction = selfTestContext.PairTest;
var removedLeafHandles = new QuickList <int>(leafCount, pool);
for (int i = 0; i < iterations; ++i)
{
var changeCount = random.Next(maximumChangesPerIteration);
for (int j = 0; j <= changeCount; ++j)
{
var addedFraction = tree.LeafCount / (float)leafCount;
if (random.NextDouble() < addedFraction)
{
//Remove a leaf.
var leafIndexToRemove = random.Next(tree.LeafCount);
var handleToRemove = leafIndexToHandle[leafIndexToRemove];
var movedLeafIndex = tree.RemoveAt(leafIndexToRemove);
if (movedLeafIndex >= 0)
{
var movedHandle = leafIndexToHandle[movedLeafIndex];
handleToLeafIndex[movedHandle] = leafIndexToRemove;
leafIndexToHandle[leafIndexToRemove] = movedHandle;
leafIndexToHandle[movedLeafIndex] = -1;
}
else
{
//The removed leaf was the last one. This leaf index is no longer associated with any existing leaf.
leafIndexToHandle[leafIndexToRemove] = -1;
}
handleToLeafIndex[handleToRemove] = -1;
removedLeafHandles.AddUnsafely(handleToRemove);
tree.Validate();
}
else
{
//Add a leaf.
var indexInRemovedList = random.Next(removedLeafHandles.Count);
var handleToAdd = removedLeafHandles[indexInRemovedList];
removedLeafHandles.FastRemoveAt(indexInRemovedList);
var leafIndex = tree.Add(ref leafBounds[handleToAdd], pool);
leafIndexToHandle[leafIndex] = handleToAdd;
handleToLeafIndex[handleToAdd] = leafIndex;
tree.Validate();
}
}
tree.Refit();
tree.Validate();
tree.RefitAndRefine(pool, i);
tree.Validate();
var handler = new OverlapHandler();
tree.GetSelfOverlaps(ref handler);
tree.Validate();
refineContext.RefitAndRefine(ref tree, pool, threadDispatcher, i);
tree.Validate();
for (int k = 0; k < threadDispatcher.ThreadCount; ++k)
{
overlapHandlers[k] = new OverlapHandler();
}
selfTestContext.PrepareJobs(ref tree, overlapHandlers, threadDispatcher.ThreadCount);
threadDispatcher.DispatchWorkers(pairTestAction);
selfTestContext.CompleteSelfTest();
tree.Validate();
if (i % 50 == 0)
{
Console.WriteLine($"Cost: {tree.MeasureCostMetric()}");
Console.WriteLine($"Cache Quality: {tree.MeasureCacheQuality()}");
Console.WriteLine($"Overlap Count: {handler.OverlapCount}");
}
}
threadDispatcher.Dispose();
pool.Clear();
}
private static void RemoveInsidePoints(ref QuickList<Vector3> points, ref QuickList<int> triangleIndices, ref QuickList<int> outsidePoints)
{
var insidePoints = new QuickList<int>(BufferPools<int>.Locking);
//We're going to remove points from this list as we go to prune it down to the truly inner points.
insidePoints.AddRange(outsidePoints);
outsidePoints.Clear();
for (int i = 0; i < triangleIndices.Count && insidePoints.Count > 0; i += 3)
{
//Compute the triangle's plane in point-normal representation to test other points against.
Vector3 normal;
FindNormal(ref triangleIndices, ref points, i, out normal);
Vector3 p = points.Elements[triangleIndices.Elements[i]];
for (int j = insidePoints.Count - 1; j >= 0; --j)
{
//Offset from the triangle to the current point, tested against the normal, determines if the current point is visible
//from the triangle face.
Vector3 offset = points.Elements[insidePoints.Elements[j]] - p;
float dot = Vector3.Dot(offset, normal);
//If it's visible, then it's outside!
if (dot > 0)
{
//This point is known to be on the outside; put it on the outside!
outsidePoints.Add(insidePoints.Elements[j]);
insidePoints.FastRemoveAt(j);
}
}
}
insidePoints.Dispose();
}
private static void MaintainEdge(int a, int b, ref QuickList<int> edges)
{
bool contained = false;
int index = 0;
for (int k = 0; k < edges.Count; k += 2)
{
if ((edges[k] == a && edges[k + 1] == b) || (edges[k] == b && edges[k + 1] == a))
{
contained = true;
index = k;
}
}
//If it isn't present, add it to the edge list.
if (!contained)
{
edges.Add(a);
edges.Add(b);
}
else
{
//If it is present, that means both edge-connected triangles were deleted now, so get rid of it.
edges.FastRemoveAt(index + 1);
edges.FastRemoveAt(index);
}
}
/// <summary>
/// Identifies the indices of points in a set which are on the outer convex hull of the set.
/// </summary>
/// <param name="points">List of points in the set.</param>
/// <param name="outputTriangleIndices">List of indices into the input point set composing the triangulated surface of the convex hull.
/// Each group of 3 indices represents a triangle on the surface of the hull.</param>
public static void GetConvexHull(ref QuickList<Vector3> points, ref QuickList<int> outputTriangleIndices)
{
if (points.Count == 0)
{
throw new ArgumentException("Point set must have volume.");
}
var outsidePoints = new QuickList<int>(BufferPools<int>.Locking, BufferPool.GetPoolIndex(points.Count - 4));
//Build the initial tetrahedron.
//It will also give us the location of a point which is guaranteed to be within the
//final convex hull. We can use this point to calibrate the winding of triangles.
//A set of outside point candidates (all points other than those composing the tetrahedron) will be returned in the outsidePoints list.
//That list will then be further pruned by the RemoveInsidePoints call.
Vector3 insidePoint;
ComputeInitialTetrahedron(ref points, ref outsidePoints, ref outputTriangleIndices, out insidePoint);
//Compute outside points.
RemoveInsidePoints(ref points, ref outputTriangleIndices, ref outsidePoints);
var edges = new QuickList<int>(BufferPools<int>.Locking);
var toRemove = new QuickList<int>(BufferPools<int>.Locking);
var newTriangles = new QuickList<int>(BufferPools<int>.Locking);
//We're now ready to begin the main loop.
while (outsidePoints.Count > 0)
{
//While the convex hull is incomplete...
for (int k = 0; k < outputTriangleIndices.Count; k += 3)
{
//Find the normal of the triangle
Vector3 normal;
FindNormal(ref outputTriangleIndices, ref points, k, out normal);
//Get the furthest point in the direction of the normal.
int maxIndexInOutsideList = GetExtremePoint(ref normal, ref points, ref outsidePoints);
int maxIndex = outsidePoints.Elements[maxIndexInOutsideList];
Vector3 maximum = points.Elements[maxIndex];
//If the point is beyond the current triangle, continue.
Vector3 offset = maximum - points.Elements[outputTriangleIndices.Elements[k]];
float dot = Vector3.Dot(normal, offset);
if (dot > 0)
{
//It's been picked! Remove the maximum point from the outside.
outsidePoints.FastRemoveAt(maxIndexInOutsideList);
//Remove any triangles that can see the point, including itself!
edges.Clear();
toRemove.Clear();
for (int n = outputTriangleIndices.Count - 3; n >= 0; n -= 3)
{
//Go through each triangle, if it can be seen, delete it and use maintainEdge on its edges.
if (IsTriangleVisibleFromPoint(ref outputTriangleIndices, ref points, n, ref maximum))
{
//This triangle can see it!
//TODO: CONSIDER CONSISTENT WINDING HAPPYTIMES
MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 1], ref edges);
MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 2], ref edges);
MaintainEdge(outputTriangleIndices[n + 1], outputTriangleIndices[n + 2], ref edges);
//Because fast removals are being used, the order is very important.
//It's pulling indices in from the end of the list in order, and also ensuring
//that we never issue a removal order beyond the end of the list.
outputTriangleIndices.FastRemoveAt(n + 2);
outputTriangleIndices.FastRemoveAt(n + 1);
outputTriangleIndices.FastRemoveAt(n);
}
}
//Create new triangles.
for (int n = 0; n < edges.Count; n += 2)
{
//For each edge, create a triangle with the extreme point.
newTriangles.Add(edges[n]);
newTriangles.Add(edges[n + 1]);
newTriangles.Add(maxIndex);
}
//Only verify the windings of the new triangles.
VerifyWindings(ref newTriangles, ref points, ref insidePoint);
outputTriangleIndices.AddRange(ref newTriangles);
newTriangles.Count = 0;
//Remove all points from the outsidePoints if they are inside the polyhedron
RemoveInsidePoints(ref points, ref outputTriangleIndices, ref outsidePoints);
//The list has been significantly messed with, so restart the loop.
break;
}
}
}
outsidePoints.Dispose();
edges.Dispose();
toRemove.Dispose();
newTriangles.Dispose();
}
/// <summary>
/// Identifies the indices of points in a set which are on the outer convex hull of the set.
/// </summary>
/// <param name="points">List of points in the set.</param>
/// <param name="outputTriangleIndices">List of indices into the input point set composing the triangulated surface of the convex hull.
/// Each group of 3 indices represents a triangle on the surface of the hull.</param>
public static void GetConvexHull(ref QuickList <Vector3> points, ref QuickList <int> outputTriangleIndices)
{
if (points.Count == 0)
{
throw new ArgumentException("Point set must have volume.");
}
var outsidePoints = new QuickList <int>(BufferPools <int> .Locking, BufferPool.GetPoolIndex(points.Count - 4));
//Build the initial tetrahedron.
//It will also give us the location of a point which is guaranteed to be within the
//final convex hull. We can use this point to calibrate the winding of triangles.
//A set of outside point candidates (all points other than those composing the tetrahedron) will be returned in the outsidePoints list.
//That list will then be further pruned by the RemoveInsidePoints call.
Vector3 insidePoint;
ComputeInitialTetrahedron(ref points, ref outsidePoints, ref outputTriangleIndices, out insidePoint);
//Compute outside points.
RemoveInsidePoints(ref points, ref outputTriangleIndices, ref outsidePoints);
var edges = new QuickList <int>(BufferPools <int> .Locking);
var toRemove = new QuickList <int>(BufferPools <int> .Locking);
var newTriangles = new QuickList <int>(BufferPools <int> .Locking);
//We're now ready to begin the main loop.
while (outsidePoints.Count > 0)
{
//While the convex hull is incomplete...
for (int k = 0; k < outputTriangleIndices.Count; k += 3)
{
//Find the normal of the triangle
Vector3 normal;
FindNormal(ref outputTriangleIndices, ref points, k, out normal);
//Get the furthest point in the direction of the normal.
int maxIndexInOutsideList = GetExtremePoint(ref normal, ref points, ref outsidePoints);
int maxIndex = outsidePoints.Elements[maxIndexInOutsideList];
Vector3 maximum = points.Elements[maxIndex];
//If the point is beyond the current triangle, continue.
Vector3 offset = maximum - points.Elements[outputTriangleIndices.Elements[k]];
float dot = Vector3.Dot(normal, offset);
if (dot > 0)
{
//It's been picked! Remove the maximum point from the outside.
outsidePoints.FastRemoveAt(maxIndexInOutsideList);
//Remove any triangles that can see the point, including itself!
edges.Clear();
toRemove.Clear();
for (int n = outputTriangleIndices.Count - 3; n >= 0; n -= 3)
{
//Go through each triangle, if it can be seen, delete it and use maintainEdge on its edges.
if (IsTriangleVisibleFromPoint(ref outputTriangleIndices, ref points, n, ref maximum))
{
//This triangle can see it!
//TODO: CONSIDER CONSISTENT WINDING HAPPYTIMES
MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 1], ref edges);
MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 2], ref edges);
MaintainEdge(outputTriangleIndices[n + 1], outputTriangleIndices[n + 2], ref edges);
//Because fast removals are being used, the order is very important.
//It's pulling indices in from the end of the list in order, and also ensuring
//that we never issue a removal order beyond the end of the list.
outputTriangleIndices.FastRemoveAt(n + 2);
outputTriangleIndices.FastRemoveAt(n + 1);
outputTriangleIndices.FastRemoveAt(n);
}
}
//Create new triangles.
for (int n = 0; n < edges.Count; n += 2)
{
//For each edge, create a triangle with the extreme point.
newTriangles.Add(edges[n]);
newTriangles.Add(edges[n + 1]);
newTriangles.Add(maxIndex);
}
//Only verify the windings of the new triangles.
VerifyWindings(ref newTriangles, ref points, ref insidePoint);
outputTriangleIndices.AddRange(ref newTriangles);
newTriangles.Count = 0;
//Remove all points from the outsidePoints if they are inside the polyhedron
RemoveInsidePoints(ref points, ref outputTriangleIndices, ref outsidePoints);
//The list has been significantly messed with, so restart the loop.
break;
}
}
}
outsidePoints.Dispose();
edges.Dispose();
toRemove.Dispose();
newTriangles.Dispose();
}