unsafe void CollectSubtreesForNodeDirect(int nodeIndex, int remainingDepth,
ref QuickList <int> subtrees, ref QuickQueue <int> internalNodes, out float treeletCost)
{
internalNodes.EnqueueUnsafely(nodeIndex);
treeletCost = 0;
ref var node = ref Nodes[nodeIndex];
unsafe void CollectSubtreesForNodeDirect(int nodeIndex, int remainingDepth,
ref QuickList <int> subtrees, ref QuickQueue <int> internalNodes, out float treeletCost)
{
internalNodes.EnqueueUnsafely(nodeIndex);
treeletCost = 0;
var node = nodes + nodeIndex;
var children = &node->A;
--remainingDepth;
if (remainingDepth >= 0)
{
for (int i = 0; i < 2; ++i)
{
ref var child = ref children[i];
if (child.Index >= 0)
{
treeletCost += ComputeBoundsMetric(ref child.Min, ref child.Max);
float childCost;
CollectSubtreesForNodeDirect(child.Index, remainingDepth, ref subtrees, ref internalNodes, out childCost);
treeletCost += childCost;
}
else
{
//It's a leaf, immediately add it to subtrees.
subtrees.AddUnsafely(child.Index);
}
}
}
public TimingsRingBuffer(int maximumCapacity)
{
if (maximumCapacity <= 0)
{
throw new ArgumentException("Capacity must be positive.");
}
QuickQueue <double, Array <double> > .Create(new PassthroughArrayPool <double>(), maximumCapacity, out queue);
}
public TimingsRingBuffer(int maximumCapacity, BufferPool pool)
{
if (maximumCapacity <= 0)
{
throw new ArgumentException("Capacity must be positive.");
}
this.pool = pool;
queue = new QuickQueue <double>(maximumCapacity, pool);
}
public unsafe void CollectSubtreesDirect(int nodeIndex, int maximumSubtrees, ref QuickList<int> subtrees, ref QuickQueue<int> internalNodes, out float treeletCost)
{
//TODO: doesn't work for non-node2.
Debug.Assert(ChildrenCapacity == 2, "If you're going to use >2ary trees, you need to correct the maximum depth. Probably better to use explicit recursion depth as input...");
var maximumDepth = BufferPool<int>.GetPoolIndex(maximumSubtrees) - 1;
Debug.Assert(maximumDepth > 0);
//Cost excludes the treelet root, since refinement can't change the treelet root's size. So don't bother including it in treeletCost.
CollectSubtreesForNodeDirect(nodeIndex, maximumDepth, ref subtrees, ref internalNodes, out treeletCost);
}
public static void TestQueueResizing <TSpan, TPool>(TPool pool)
where TSpan : ISpan <int>
where TPool : IMemoryPool <int, TSpan>
{
Random random = new Random(5);
QuickQueue <int, TSpan> .Create(pool, 4, out var queue);
Queue <int> controlQueue = new Queue <int>();
for (int iterationIndex = 0; iterationIndex < 1000000; ++iterationIndex)
{
if (random.NextDouble() < 0.7)
{
queue.Enqueue(iterationIndex, pool);
controlQueue.Enqueue(iterationIndex);
}
if (random.NextDouble() < 0.2)
{
queue.Dequeue();
controlQueue.Dequeue();
}
if (iterationIndex % 1000 == 0)
{
queue.EnsureCapacity(queue.Count * 3, pool);
}
else if (iterationIndex % 7777 == 0)
{
queue.Compact(pool);
}
}
Debug.Assert(queue.Count == controlQueue.Count, "e");
while (queue.Count > 0)
{
var a = queue.Dequeue();
var b = controlQueue.Dequeue();
Debug.Assert(a == b);
Debug.Assert(queue.Count == controlQueue.Count);
}
queue.Dispose(pool);
}
public static void TestQueueResizing(IUnmanagedMemoryPool pool)
{
Random random = new Random(5);
var queue = new QuickQueue <int>(4, pool);
Queue <int> controlQueue = new Queue <int>();
for (int iterationIndex = 0; iterationIndex < 1000000; ++iterationIndex)
{
if (random.NextDouble() < 0.7)
{
queue.Enqueue(iterationIndex, pool);
controlQueue.Enqueue(iterationIndex);
}
if (random.NextDouble() < 0.2)
{
queue.Dequeue();
controlQueue.Dequeue();
}
if (iterationIndex % 1000 == 0)
{
queue.EnsureCapacity(queue.Count * 3, pool);
}
else if (iterationIndex % 7777 == 0)
{
queue.Compact(pool);
}
}
Debug.Assert(queue.Count == controlQueue.Count, "e");
while (queue.Count > 0)
{
var a = queue.Dequeue();
var b = controlQueue.Dequeue();
Debug.Assert(a == b);
Debug.Assert(queue.Count == controlQueue.Count);
}
queue.Dispose(pool);
}
unsafe void CollectSubtreesForNodeDirect(int nodeIndex, int remainingDepth, ref QuickList<int> subtrees, ref QuickQueue<int> internalNodes, out float treeletCost)
{
internalNodes.Enqueue(nodeIndex);
treeletCost = 0;
var node = nodes + nodeIndex;
var children = &node->ChildA;
var bounds = &node->A;
--remainingDepth;
if (remainingDepth >= 0)
{
for (int i = 0; i < node->ChildCount; ++i)
{
if (children[i] >= 0)
{
treeletCost += ComputeBoundsMetric(ref bounds[i]);
float childCost;
CollectSubtreesForNodeDirect(children[i], remainingDepth, ref subtrees, ref internalNodes, out childCost);
treeletCost += childCost;
}
else
{
//It's a leaf, immediately add it to subtrees.
subtrees.Add(children[i]);
}
}
}
else
{
//Recursion has bottomed out. Add every child.
//Once again, note that the treelet costs of these nodes are not considered, even if they are internal.
//That's because the subtree internal nodes cannot change size due to the refinement.
for (int i = 0; i < node->ChildCount; ++i)
{
subtrees.Add(children[i]);
}
}
}
public static void TestQueueResizing()
{
Random random = new Random(5);
UnsafeBufferPool <int> pool = new UnsafeBufferPool <int>();
QuickQueue <int> queue = new QuickQueue <int>(pool, 2);
Queue <int> controlQueue = new Queue <int>();
for (int iterationIndex = 0; iterationIndex < 1000000; ++iterationIndex)
{
if (random.NextDouble() < 0.7)
{
queue.Enqueue(iterationIndex);
controlQueue.Enqueue(iterationIndex);
}
if (random.NextDouble() < 0.2)
{
queue.Dequeue();
controlQueue.Dequeue();
}
if (iterationIndex % 1000 == 0)
{
queue.EnsureCapacity(queue.Count * 3);
}
else if (iterationIndex % 7777 == 0)
{
queue.Compact();
}
}
Assert.IsTrue(queue.Count == controlQueue.Count);
while (queue.Count > 0)
{
var a = queue.Dequeue();
var b = controlQueue.Dequeue();
Assert.IsTrue(a == b);
Assert.IsTrue(queue.Count == controlQueue.Count);
}
}
public unsafe void CollectSubtreesDirect(int nodeIndex, int maximumSubtrees, ref QuickList <int> subtrees, ref QuickQueue <int> internalNodes, out float treeletCost)
{
//TODO: doesn't work for non-node2.
Debug.Assert(ChildrenCapacity == 2, "If you're going to use >2ary trees, you need to correct the maximum depth. Probably better to use explicit recursion depth as input...");
var maximumDepth = BufferPool <int> .GetPoolIndex(maximumSubtrees) - 1;
Debug.Assert(maximumDepth > 0);
//Cost excludes the treelet root, since refinement can't change the treelet root's size. So don't bother including it in treeletCost.
CollectSubtreesForNodeDirect(nodeIndex, maximumDepth, ref subtrees, ref internalNodes, out treeletCost);
}
unsafe void CollectSubtreesForNodeDirect(int nodeIndex, int remainingDepth, ref QuickList <int> subtrees, ref QuickQueue <int> internalNodes, out float treeletCost)
{
internalNodes.Enqueue(nodeIndex);
treeletCost = 0;
var node = nodes + nodeIndex;
var children = &node->ChildA;
var bounds = &node->A;
--remainingDepth;
if (remainingDepth >= 0)
{
for (int i = 0; i < node->ChildCount; ++i)
{
if (children[i] >= 0)
{
treeletCost += ComputeBoundsMetric(ref bounds[i]);
float childCost;
CollectSubtreesForNodeDirect(children[i], remainingDepth, ref subtrees, ref internalNodes, out childCost);
treeletCost += childCost;
}
else
{
//It's a leaf, immediately add it to subtrees.
subtrees.Add(children[i]);
}
}
}
else
{
//Recursion has bottomed out. Add every child.
//Once again, note that the treelet costs of these nodes are not considered, even if they are internal.
//That's because the subtree internal nodes cannot change size due to the refinement.
for (int i = 0; i < node->ChildCount; ++i)
{
subtrees.Add(children[i]);
}
}
}
public unsafe override void Initialize(Camera camera)
{
camera.Position = new Vector3(-3f, 3, -3f);
camera.Yaw = MathHelper.Pi * 3f / 4;
camera.Pitch = MathHelper.Pi * 0.1f;
Simulation = Simulation.Create(BufferPool, new TestCallbacks(),
new SimulationAllocationSizes
{
Bodies = 1,
ConstraintCountPerBodyEstimate = 1,
Constraints = 1,
ConstraintsPerTypeBatch = 1,
Islands = 1,
ShapesPerType = 1,
Statics = 1
});
Simulation.PoseIntegrator.Gravity = new Vector3(0, -10, 0);
Simulation.Deterministic = false;
var staticShape = new Sphere(6);
var staticShapeIndex = Simulation.Shapes.Add(ref staticShape);
const int staticGridWidthInSpheres = 128;
const float staticSpacing = 8;
for (int i = 0; i < staticGridWidthInSpheres; ++i)
{
for (int j = 0; j < staticGridWidthInSpheres; ++j)
{
var staticDescription = new StaticDescription
{
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
Shape = staticShapeIndex,
SpeculativeMargin = 0.1f
},
Pose = new RigidPose
{
Position = new Vector3(
-staticGridWidthInSpheres * staticSpacing * 0.5f + i * staticSpacing,
-4 + 4 * (float)Math.Cos(i * 0.3) + 4 * (float)Math.Cos(j * 0.3),
-staticGridWidthInSpheres * staticSpacing * 0.5f + j * staticSpacing),
Orientation = BepuUtilities.Quaternion.Identity
}
};
Simulation.Statics.Add(ref staticDescription);
}
}
//A bunch of kinematic balls do acrobatics as an extra stressor.
var kinematicShape = new Sphere(8);
var kinematicShapeIndex = Simulation.Shapes.Add(ref staticShape);
var kinematicCount = 64;
var anglePerKinematic = MathHelper.TwoPi / kinematicCount;
var startingRadius = 256;
kinematicHandles = new int[kinematicCount];
for (int i = 0; i < kinematicCount; ++i)
{
var angle = anglePerKinematic * i;
var description = new BodyDescription
{
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
Shape = kinematicShapeIndex,
SpeculativeMargin = 0.1f
},
Pose = new RigidPose
{
Position = new Vector3(
startingRadius * (float)Math.Cos(angle),
0,
startingRadius * (float)Math.Sin(angle)),
Orientation = BepuUtilities.Quaternion.Identity
},
Activity = new BodyActivityDescription {
DeactivationThreshold = 0, MinimumTimestepCountUnderThreshold = 4
},
};
kinematicHandles[i] = Simulation.Bodies.Add(ref description);
}
QuickQueue <int, Buffer <int> > .Create(BufferPool.SpecializeFor <int>(), 65536, out dynamicHandles);
random = new Random(5);
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-15f, 20, -15f);
camera.Yaw = MathHelper.Pi * 3f / 4;
camera.Pitch = MathHelper.Pi * 0.1f;
//Using minimum sized allocations forces as many resizes as possible.
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), initialAllocationSizes:
new SimulationAllocationSizes
{
Bodies = 1,
ConstraintCountPerBodyEstimate = 1,
Constraints = 1,
ConstraintsPerTypeBatch = 1,
Islands = 1,
ShapesPerType = 1,
Statics = 1
});
Simulation.Deterministic = true;
const int planeWidth = 8;
const int planeHeight = 8;
DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
(int x, int y) =>
{
Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2);
return(new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.2f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y));
}, new Vector3(2, 1, 2), BufferPool, out var staticShape);
var staticShapeIndex = Simulation.Shapes.Add(staticShape);
const int staticGridWidthInInstances = 128;
const float staticSpacing = 8;
for (int i = 0; i < staticGridWidthInInstances; ++i)
{
for (int j = 0; j < staticGridWidthInInstances; ++j)
{
var staticDescription = new StaticDescription
{
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
Shape = staticShapeIndex,
SpeculativeMargin = 0.1f
},
Pose = new RigidPose
{
Position = new Vector3(
-staticGridWidthInInstances * staticSpacing * 0.5f + i * staticSpacing,
-4 + 4 * (float)Math.Cos(i * 0.3) + 4 * (float)Math.Cos(j * 0.3),
-staticGridWidthInInstances * staticSpacing * 0.5f + j * staticSpacing),
Orientation = Quaternion.Identity
}
};
Simulation.Statics.Add(staticDescription);
}
}
//A bunch of kinematic balls do acrobatics as an extra stressor.
var kinematicShape = new Sphere(8);
var kinematicShapeIndex = Simulation.Shapes.Add(kinematicShape);
var kinematicCount = 64;
var anglePerKinematic = MathHelper.TwoPi / kinematicCount;
var startingRadius = 256;
kinematicHandles = new BodyHandle[kinematicCount];
for (int i = 0; i < kinematicCount; ++i)
{
var angle = anglePerKinematic * i;
var description = new BodyDescription
{
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
Shape = kinematicShapeIndex,
SpeculativeMargin = 0.1f
},
Pose = new RigidPose
{
Position = new Vector3(
startingRadius * (float)Math.Cos(angle),
0,
startingRadius * (float)Math.Sin(angle)),
Orientation = Quaternion.Identity
},
Activity = new BodyActivityDescription {
SleepThreshold = 0, MinimumTimestepCountUnderThreshold = 4
},
};
kinematicHandles[i] = Simulation.Bodies.Add(description);
}
dynamicHandles = new QuickQueue <BodyHandle>(65536, BufferPool);
removedStatics = new QuickQueue <StaticDescription>(512, BufferPool);
random = new Random(5);
}
public static void TestQueueResizing()
{
Random random = new Random(5);
UnsafeBufferPool<int> pool = new UnsafeBufferPool<int>();
QuickQueue<int> queue = new QuickQueue<int>(pool, 2);
Queue<int> controlQueue = new Queue<int>();
for (int iterationIndex = 0; iterationIndex < 1000000; ++iterationIndex)
{
if (random.NextDouble() < 0.7)
{
queue.Enqueue(iterationIndex);
controlQueue.Enqueue(iterationIndex);
}
if (random.NextDouble() < 0.2)
{
queue.Dequeue();
controlQueue.Dequeue();
}
if (iterationIndex % 1000 == 0)
{
queue.EnsureCapacity(queue.Count * 3);
}
else if (iterationIndex % 7777 == 0)
{
queue.Compact();
}
}
Assert.IsTrue(queue.Count == controlQueue.Count);
while (queue.Count > 0)
{
var a = queue.Dequeue();
var b = controlQueue.Dequeue();
Assert.IsTrue(a == b);
Assert.IsTrue(queue.Count == controlQueue.Count);
}
}
public unsafe void GetSelfOverlapsViaStreamingQueries <TResultList>(ref TResultList results) where TResultList : IList <Overlap>
{
//var startTime = Stopwatch.GetTimestamp();
var rootTarget = new StreamingTarget {
LeafGroups = new QuickList <StreamingLeafGroup>(BufferPools <StreamingLeafGroup> .Locking, BufferPool <StreamingLeafGroup> .GetPoolIndex(LeafCount))
};
rootTarget.LeafGroups.Add(new StreamingLeafGroup());
for (int i = 0; i < LeafCount; ++i)
{
BoundingBoxWide leafWide;
BoundingBoxWide.GetBoundingBox(ref Levels[leaves[i].LevelIndex].Nodes[leaves[i].NodeIndex].BoundingBoxes, leaves[i].ChildIndex, out leafWide);
var leafIndexWide = new Vector <int>(i);
rootTarget.Add(ref leafIndexWide, ref leafWide, singleMasks);
}
//var endTime = Stopwatch.GetTimestamp();
//Console.WriteLine($"Initial target construction time: {(endTime - startTime) / (double)Stopwatch.Frequency}");
QuickQueue <StreamingTarget> targets = new QuickQueue <StreamingTarget>(BufferPools <StreamingTarget> .Locking, BufferPool <StreamingLeafGroup> .GetPoolIndex(LeafCount));
targets.Enqueue(ref rootTarget);
QuickList <int> fallbackResults = new QuickList <int>(BufferPools <int> .Locking);
StreamingTarget target;
while (targets.TryDequeueLast(out target))
{
const int GroupFallbackThreshold = 2; //unfortunately, this should be as high as possible right now because the regular query is faster, period.
if (target.LeafGroups.Count <= GroupFallbackThreshold)
{
var max = target.LastLeavesCount == Vector <int> .Count ? target.LeafGroups.Count : target.LeafGroups.Count - 1;
for (int leafGroupIndex = 0; leafGroupIndex < max; ++leafGroupIndex)
{
for (int leafInGroupIndex = 0; leafInGroupIndex < Vector <int> .Count; ++leafInGroupIndex)
{
BoundingBoxWide leafWide;
BoundingBoxWide.GetBoundingBox(ref target.LeafGroups.Elements[leafGroupIndex].BoundingBoxes, leafInGroupIndex, out leafWide);
TestRecursive(target.LevelIndex, target.NodeIndex, ref leafWide, ref fallbackResults);
for (int resultIndex = 0; resultIndex < fallbackResults.Count; ++resultIndex)
{
var queryLeafIndex = target.LeafGroups.Elements[leafGroupIndex].Leaves[leafInGroupIndex];
if (queryLeafIndex < fallbackResults.Elements[resultIndex])
{
results.Add(new Overlap {
A = queryLeafIndex, B = fallbackResults.Elements[resultIndex]
});
}
}
fallbackResults.Count = 0;
}
}
if (target.LastLeavesCount < Vector <int> .Count)
{
var leafGroupIndex = target.LeafGroups.Count - 1;
for (int leafInGroupIndex = 0; leafInGroupIndex < target.LastLeavesCount; ++leafInGroupIndex)
{
BoundingBoxWide leafWide;
BoundingBoxWide.GetBoundingBox(ref target.LeafGroups.Elements[leafGroupIndex].BoundingBoxes, leafInGroupIndex, out leafWide);
TestRecursive(target.LevelIndex, target.NodeIndex, ref leafWide, ref fallbackResults);
for (int resultIndex = 0; resultIndex < fallbackResults.Count; ++resultIndex)
{
var queryLeafIndex = target.LeafGroups.Elements[leafGroupIndex].Leaves[leafInGroupIndex];
if (queryLeafIndex < fallbackResults.Elements[resultIndex])
{
results.Add(new Overlap {
A = queryLeafIndex, B = fallbackResults.Elements[resultIndex]
});
}
}
fallbackResults.Count = 0;
}
}
}
else
{
var node = Levels[target.LevelIndex].Nodes[target.NodeIndex];
//Test each node child against all of the leaves for this node.
for (int nodeChildIndex = 0; nodeChildIndex < Vector <int> .Count; ++nodeChildIndex)
{
if (node.Children[nodeChildIndex] == -1)
{
continue;
}
BoundingBoxWide nodeChildWide;
BoundingBoxWide.GetBoundingBox(ref node.BoundingBoxes, nodeChildIndex, out nodeChildWide);
if (node.Children[nodeChildIndex] >= 0)
{
//Internal node. Can spawn more targets.
StreamingTarget newTarget = new StreamingTarget
{
LevelIndex = target.LevelIndex + 1,
NodeIndex = node.Children[nodeChildIndex],
LeafGroups = new QuickList <StreamingLeafGroup>(BufferPools <StreamingLeafGroup> .Locking, BufferPool <StreamingLeafGroup> .GetPoolIndex(target.LeafGroups.Count))
};
newTarget.LeafGroups.Add(new StreamingLeafGroup());
for (int leafGroupIndex = 0; leafGroupIndex < target.LeafGroups.Count; ++leafGroupIndex)
{
Vector <int> intersectionMask;
BoundingBoxWide.Intersects(ref nodeChildWide, ref target.LeafGroups.Elements[leafGroupIndex].BoundingBoxes, out intersectionMask);
int leafCountInGroup = leafGroupIndex == target.LeafGroups.Count - 1 ? target.LastLeavesCount : Vector <int> .Count;
for (int leafIndexInGroup = 0; leafIndexInGroup < leafCountInGroup; ++leafIndexInGroup)
{
if (intersectionMask[leafIndexInGroup] < 0)
{
newTarget.Add(ref target, leafGroupIndex, leafIndexInGroup, singleMasks);
}
}
}
targets.Enqueue(ref newTarget);
}
else
{
//Leaf node.
var nodeLeafIndex = Encode(node.Children[nodeChildIndex]);
for (int leafGroupIndex = 0; leafGroupIndex < target.LeafGroups.Count; ++leafGroupIndex)
{
Vector <int> intersectionMask;
BoundingBoxWide.Intersects(ref nodeChildWide, ref target.LeafGroups.Elements[leafGroupIndex].BoundingBoxes, out intersectionMask);
int leafCountInGroup = leafGroupIndex == target.LeafGroups.Count - 1 ? target.LastLeavesCount : Vector <int> .Count;
for (int leafIndexInGroup = 0; leafIndexInGroup < leafCountInGroup; ++leafIndexInGroup)
{
if (intersectionMask[leafIndexInGroup] < 0)
{
var leafIndex = target.LeafGroups[leafGroupIndex].Leaves[leafIndexInGroup];
if (leafIndex < nodeLeafIndex) //The other leaf will also find a collision!
{
results.Add(new Overlap {
A = leafIndex, B = nodeLeafIndex
});
}
}
}
}
}
}
}
target.LeafGroups.Count = 0; //Don't bother forcing a clear on these. TODO: buffer safety check disable
target.LeafGroups.Dispose();
}
targets.Dispose();
fallbackResults.Dispose();
//Console.WriteLine("Streaming Query based results:");
//for (int i = 0; i < results.Count; ++i)
//{
// Console.WriteLine($"{results[i].A}, {results[i].B}");
//}
}
