public void GetEntries(BoundingSphere boundingShape, IList <BroadPhaseEntry> overlaps)
{
//Create a bounding box based on the bounding sphere.
//Compute the min and max of the bounding box.
//Loop through the cells and select bounding boxes which overlap the x axis.
#if !WINDOWS
Vector3 offset = new Vector3();
#else
Vector3 offset;
#endif
offset.X = boundingShape.Radius;
offset.Y = offset.X;
offset.Z = offset.Y;
BoundingBox box;
Vector3.Add(ref boundingShape.Center, ref offset, out box.Max);
Vector3.Subtract(ref boundingShape.Center, ref offset, out box.Min);
Int2 min, max;
Grid2DSortAndSweep.ComputeCell(ref box.Min, out min);
Grid2DSortAndSweep.ComputeCell(ref box.Max, out max);
for (int i = min.Y; i <= max.Y; i++)
{
for (int j = min.Z; j <= max.Z; j++)
{
//Grab the cell that we are currently in.
Int2 cellIndex;
cellIndex.Y = i;
cellIndex.Z = j;
GridCell2D cell;
if (owner.cellSet.TryGetCell(ref cellIndex, out cell))
{
//To fully accelerate this, the entries list would need to contain both min and max interval markers.
//Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list.
//Consider some giant bounding box that spans the entire list.
for (int k = 0; k < cell.entries.Count &&
cell.entries.Elements[k].item.boundingBox.Min.X <= box.Max.X; k++) //TODO: Try additional x axis pruning? A bit of optimization potential due to overlap with AABB test.
{
bool intersects;
var item = cell.entries.Elements[k].item;
item.boundingBox.Intersects(ref boundingShape, out intersects);
if (intersects && !overlaps.Contains(item))
{
overlaps.Add(item);
}
}
}
}
}
}
internal void Initialize(BroadPhaseEntry entry)
{
this.item = entry;
Grid2DSortAndSweep.ComputeCell(ref entry.boundingBox.Min, out previousMin);
Grid2DSortAndSweep.ComputeCell(ref entry.boundingBox.Max, out previousMax);
}
public bool RayCast(Ray ray, float maximumLength, IList <BroadPhaseEntry> outputIntersections)
{
if (maximumLength == float.MaxValue)
{
throw new NotSupportedException("The Grid2DSortAndSweep broad phase cannot accelerate infinite ray casts. Consider specifying a maximum length or using a broad phase which supports infinite ray casts.");
}
//Use 2d line rasterization.
//Compute the exit location in the cell.
//Test against each bounding box up until the exit value is reached.
float length = 0;
Int2 cellIndex;
Vector3 currentPosition = ray.Position;
Grid2DSortAndSweep.ComputeCell(ref currentPosition, out cellIndex);
while (true)
{
float cellWidth = 1 / Grid2DSortAndSweep.cellSizeInverse;
float nextT; //Distance along ray to next boundary.
float nextTy; //Distance along ray to next boundary along y axis.
float nextTz; //Distance along ray to next boundary along z axis.
//Find the next cell.
if (ray.Direction.Y > 0)
{
nextTy = ((cellIndex.Y + 1) * cellWidth - currentPosition.Y) / ray.Direction.Y;
}
else if (ray.Direction.Y < 0)
{
nextTy = ((cellIndex.Y) * cellWidth - currentPosition.Y) / ray.Direction.Y;
}
else
{
nextTy = 10e10f;
}
if (ray.Direction.Z > 0)
{
nextTz = ((cellIndex.Z + 1) * cellWidth - currentPosition.Z) / ray.Direction.Z;
}
else if (ray.Direction.Z < 0)
{
nextTz = ((cellIndex.Z) * cellWidth - currentPosition.Z) / ray.Direction.Z;
}
else
{
nextTz = 10e10f;
}
bool yIsMinimum = nextTy < nextTz;
nextT = yIsMinimum ? nextTy : nextTz;
//Grab the cell that we are currently in.
GridCell2D cell;
if (owner.cellSet.TryGetCell(ref cellIndex, out cell))
{
float endingX;
if (ray.Direction.X < 0)
{
endingX = currentPosition.X;
}
else
{
endingX = currentPosition.X + ray.Direction.X * nextT;
}
//To fully accelerate this, the entries list would need to contain both min and max interval markers.
//Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list.
//Consider some giant bounding box that spans the entire list.
for (int i = 0; i < cell.entries.Count &&
cell.entries.Elements[i].item.boundingBox.Min.X <= endingX; i++) //TODO: Try additional x axis pruning?
{
float?intersects;
var item = cell.entries.Elements[i].item;
ray.Intersects(ref item.boundingBox, out intersects);
if (intersects != null && intersects < maximumLength && !outputIntersections.Contains(item))
{
outputIntersections.Add(item);
}
}
}
//Move the position forward.
length += nextT;
if (length > maximumLength) //Note that this catches the case in which the ray is pointing right down the middle of a row (resulting in a nextT of 10e10f).
{
break;
}
Vector3 offset;
Vector3.Multiply(ref ray.Direction, nextT, out offset);
Vector3.Add(ref offset, ref currentPosition, out currentPosition);
if (yIsMinimum)
{
if (ray.Direction.Y < 0)
{
cellIndex.Y -= 1;
}
else
{
cellIndex.Y += 1;
}
}
else
if (ray.Direction.Z < 0)
{
cellIndex.Z -= 1;
}
else
{
cellIndex.Z += 1;
}
}
return(outputIntersections.Count > 0);
}
public Grid2DSortAndSweepQueryAccelerator(Grid2DSortAndSweep owner)
{
this.owner = owner;
}
internal void UpdateOverlaps(Grid2DSortAndSweep owner)
{
//Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary.
for (int i = 1; i < entries.Count; i++)
{
var entry = entries.Elements[i];
for (int j = i - 1; j >= 0; j--)
{
if (entry.item.boundingBox.Min.X < entries.Elements[j].item.boundingBox.Min.X)
{
entries.Elements[j + 1] = entries.Elements[j];
entries.Elements[j] = entry;
}
else
{
break;
}
}
}
//Sweep the list looking for overlaps.
for (int i = 0; i < entries.Count; i++)
{
Grid2DEntry a = entries.Elements[i];
Grid2DEntry b = a;
//TODO: Microoptimize
for (int j = i + 1; j < entries.Count && a.item.boundingBox.Max.X >= (b = entries.Elements[j]).item.boundingBox.Min.X; j++)
{
if (!(a.item.boundingBox.Min.Y > b.item.boundingBox.Max.Y || a.item.boundingBox.Max.Y < b.item.boundingBox.Min.Y ||
a.item.boundingBox.Min.Z > b.item.boundingBox.Max.Z || a.item.boundingBox.Max.Z < b.item.boundingBox.Min.Z))
{
//Now we know this pair is overlapping, but we do not know if this overlap is already added.
//Rather than use a hashset or other heavy structure to check, rely on the rules of the grid.
//It's possible to avoid adding pairs entirely unless we are the designated 'responsible' cell.
//All other cells will defer to the cell 'responsible' for a pair.
//A simple rule for determining the cell which is responsible is to choose the cell which is the
//smallest index in the shared cells. So first, compute that cell's index.
Int2 minimumSharedIndex = a.previousMin;
if (minimumSharedIndex.Y < b.previousMin.Y)
{
minimumSharedIndex.Y = b.previousMin.Y;
}
if (minimumSharedIndex.Y > b.previousMax.Y)
{
minimumSharedIndex.Y = b.previousMax.Y;
}
if (minimumSharedIndex.Z < b.previousMin.Z)
{
minimumSharedIndex.Z = b.previousMin.Z;
}
if (minimumSharedIndex.Z > b.previousMax.Z)
{
minimumSharedIndex.Z = b.previousMax.Z;
}
//Is our cell the minimum cell?
if (minimumSharedIndex.Y == cellIndex.Y && minimumSharedIndex.Z == cellIndex.Z)
{
owner.TryToAddOverlap(a.item, b.item);
}
}
}
}
}