internal bool TryGetIndex(ref Int2 cellIndex, out int index, out int sortingHash)
{
sortingHash = cellIndex.GetSortingHash();
int minIndex = 0; //inclusive
int maxIndex = count; //exclusive
index = 0;
while (maxIndex - minIndex > 0) //If the testing interval has a length of zero, we've done as much as we can.
{
index = (maxIndex + minIndex) / 2;
if (cells.Elements[index].sortingHash > sortingHash)
maxIndex = index;
else if (cells.Elements[index].sortingHash < sortingHash)
minIndex = ++index;
else
{
//Found an equal sorting hash!
//The hash can collide, and we cannot add an entry to
//an incorrect index. It would break the 'cell responsibility'
//used by the cell update process to avoid duplicate overlaps.
//So, check if the index we found is ACTUALLY correct.
if (cells.Elements[index].cellIndex.Y == cellIndex.Y && cells.Elements[index].cellIndex.Z == cellIndex.Z)
{
return true;
}
//If it was not the correct index, let it continue searching.
}
}
return false;
}
internal void Add(ref Int2 index, Grid2DEntry entry)
{
int cellIndex;
int sortingHash;
if (TryGetIndex(ref index, out cellIndex, out sortingHash))
{
cells.Elements[cellIndex].Add(entry);
return;
}
var cell = cellPool.Take();
cell.Initialize(ref index, sortingHash);
cell.Add(entry);
cells.Insert(cellIndex, cell);
count++;
return;
////Take an index. See if it's taken in the set.
////If it's already there, then add the entry to the cell.
////If it's not already there, create a new cell and add the entry to the cell and insert it at the index located.
//int sortingHash = index.GetSortingHash();
//int minIndex = 0; //inclusive
//int maxIndex = count; //exclusive
//int i = 0;
//while (maxIndex - minIndex > 0) //If the testing interval has a length of zero, we've done as much as we can.
//{
// i = (maxIndex + minIndex) / 2;
// if (cells.Elements[i].sortingHash > sortingHash)
// maxIndex = i;
// else if (cells.Elements[i].sortingHash < sortingHash)
// minIndex = ++i;
// else
// {
// //Found an equal sorting hash!
// //The hash can collide, and we cannot add an entry to
// //an incorrect index. It would break the 'cell responsibility'
// //used by the cell update process to avoid duplicate overlaps.
// //So, check if the index we found is ACTUALLY correct.
// if (cells.Elements[i].cellIndex.Y == index.Y && cells.Elements[i].cellIndex.Z == index.Z)
// {
// cells.Elements[i].Add(entry);
// return;
// }
// //If it was not the correct index, let it continue searching.
// }
//}
//var cell = cellPool.Take();
//cell.Initialize(ref index, sortingHash);
//cell.Add(entry);
//cells.Insert(i, cell);
//count++;
}
internal bool TryGetCell(ref Int2 cellIndex, out GridCell2D cell)
{
int index;
int sortingHash;
if (TryGetIndex(ref cellIndex, out index, out sortingHash))
{
cell = cells.Elements[index];
return true;
}
cell = null;
return false;
}
internal void Initialize(ref Int2 cellIndex, int hash)
{
this.cellIndex = cellIndex;
sortingHash = hash;
}
void UpdateEntry(int i)
{
//Compute the current cells occupied by the entry.
var entry = entries.Elements[i];
Int2 min, max;
ComputeCell(ref entry.item.boundingBox.Min, out min);
ComputeCell(ref entry.item.boundingBox.Max, out max);
//For any cell that used to be occupied (defined by the previous min/max),
//remove the entry.
for (int j = entry.previousMin.Y; j <= entry.previousMax.Y; j++)
{
for (int k = entry.previousMin.Z; k <= entry.previousMax.Z; k++)
{
if (j >= min.Y && j <= max.Y && k >= min.Z && k <= max.Z)
continue; //This cell is currently occupied, do not remove.
var index = new Int2 {Y = j, Z = k};
cellSetLocker.Enter();
cellSet.Remove(ref index, entry);
cellSetLocker.Exit();
}
}
//For any cell that is newly occupied (was not previously contained),
//add the entry.
for (int j = min.Y; j <= max.Y; j++)
{
for (int k = min.Z; k <= max.Z; k++)
{
if (j >= entry.previousMin.Y && j <= entry.previousMax.Y && k >= entry.previousMin.Z && k <= entry.previousMax.Z)
continue; //This cell is already occupied, do not add.
var index = new Int2 {Y = j, Z = k};
cellSetLocker.Enter();
cellSet.Add(ref index, entry);
cellSetLocker.Exit();
}
}
entry.previousMin = min;
entry.previousMax = max;
}
protected override void UpdateSingleThreaded()
{
lock (Locker)
{
Overlaps.Clear();
//Update the placement of objects.
for (int i = 0; i < entries.Count; i++)
{
//Compute the current cells occupied by the entry.
var entry = entries.Elements[i];
Int2 min, max;
ComputeCell(ref entry.item.boundingBox.Min, out min);
ComputeCell(ref entry.item.boundingBox.Max, out max);
//For any cell that used to be occupied (defined by the previous min/max),
//remove the entry.
for (int j = entry.previousMin.Y; j <= entry.previousMax.Y; j++)
{
for (int k = entry.previousMin.Z; k <= entry.previousMax.Z; k++)
{
if (j >= min.Y && j <= max.Y && k >= min.Z && k <= max.Z)
continue; //This cell is currently occupied, do not remove.
var index = new Int2 {Y = j, Z = k};
cellSet.Remove(ref index, entry);
}
}
//For any cell that is newly occupied (was not previously contained),
//add the entry.
for (int j = min.Y; j <= max.Y; j++)
{
for (int k = min.Z; k <= max.Z; k++)
{
if (j >= entry.previousMin.Y && j <= entry.previousMax.Y && k >= entry.previousMin.Z && k <= entry.previousMax.Z)
continue; //This cell is already occupied, do not add.
var index = new Int2 {Y = j, Z = k};
cellSet.Add(ref index, entry);
}
}
entry.previousMin = min;
entry.previousMax = max;
}
//Update each cell to find the overlaps.
for (int i = 0; i < cellSet.count; i++)
{
cellSet.cells.Elements[i].UpdateOverlaps(this);
}
}
}
internal static void ComputeCell(ref System.Numerics.Vector3 v, out Int2 cell)
{
cell.Y = (int)Math.Floor(v.Y * cellSizeInverse);
cell.Z = (int)Math.Floor(v.Z * cellSizeInverse);
}
/// <summary>
/// Removes an entry from the broad phase.
/// </summary>
/// <param name="entry">Entry to remove.</param>
public override void Remove(BroadPhaseEntry entry)
{
base.Remove(entry);
for (int i = 0; i < entries.Count; i++)
{
if (entries.Elements[i].item == entry)
{
var gridEntry = entries.Elements[i];
entries.RemoveAt(i);
//Remove the object from any cells that it is held by.
for (int j = gridEntry.previousMin.Y; j <= gridEntry.previousMax.Y; j++)
{
for (int k = gridEntry.previousMin.Z; k <= gridEntry.previousMax.Z; k++)
{
var index = new Int2 {Y = j, Z = k};
cellSet.Remove(ref index, gridEntry);
}
}
gridEntry.item = null;
entryPool.GiveBack(gridEntry);
return;
}
}
}
/// <summary>
/// Adds an entry to the broad phase.
/// </summary>
/// <param name="entry">Entry to add.</param>
public override void Add(BroadPhaseEntry entry)
{
base.Add(entry);
//Entities do not set up their own bounding box before getting stuck in here. If they're all zeroed out, the tree will be horrible.
System.Numerics.Vector3 offset;
Vector3Ex.Subtract(ref entry.boundingBox.Max, ref entry.boundingBox.Min, out offset);
if (offset.X * offset.Y * offset.Z == 0)
entry.UpdateBoundingBox();
var newEntry = entryPool.Take();
newEntry.Initialize(entry);
entries.Add(newEntry);
//Add the object to the grid.
for (int i = newEntry.previousMin.Y; i <= newEntry.previousMax.Y; i++)
{
for (int j = newEntry.previousMin.Z; j <= newEntry.previousMax.Z; j++)
{
var index = new Int2 {Y = i, Z = j};
cellSet.Add(ref index, newEntry);
}
}
}
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;
//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);
}
}
}
}
}
internal void Initialize(ref Int2 cellIndex, int hash)
{
this.cellIndex = cellIndex;
sortingHash = hash;
}
internal static void ComputeCell(ref Vector3 v, out Int2 cell)
{
cell.Y = (int)Math.Floor(v.Y * cellSizeInverse);
cell.Z = (int)Math.Floor(v.Z * cellSizeInverse);
}
internal void Remove(ref Int2 index, Grid2DEntry entry)
{
int cellIndex;
int sortingHash;
if (TryGetIndex(ref index, out cellIndex, out sortingHash))
{
cells.Elements[cellIndex].Remove(entry);
if (cells.Elements[cellIndex].entries.Count == 0)
{
//The cell is now empty. Give it back to the pool.
var toRemove = cells.Elements[cellIndex];
//There's no cleanup to do on the grid cell.
//Its list is empty, and the rest is just value types.
cells.RemoveAt(cellIndex);
cellPool.GiveBack(toRemove);
count--;
}
}
//int sortingHash = index.GetSortingHash();
//int minIndex = 0; //inclusive
//int maxIndex = count; //exclusive
//int i = 0;
//while (maxIndex - minIndex > 0) //If the testing interval has a length of zero, we've done as much as we can.
//{
// i = (maxIndex + minIndex) / 2;
// if (cells.Elements[i].sortingHash > sortingHash)
// maxIndex = i;
// else if (cells.Elements[i].sortingHash < sortingHash)
// minIndex = ++i;
// else
// {
// //Found an equal sorting hash!
// //The hash can collide, and we cannot add an entry to
// //an incorrect index. It would break the 'cell responsibility'
// //used by the cell update process to avoid duplicate overlaps.
// //So, check if the index we found is ACTUALLY correct.
// if (cells.Elements[i].cellIndex.Y == index.Y && cells.Elements[i].cellIndex.Z == index.Z)
// {
// cells.Elements[i].Remove(entry);
// if (cells.Elements[i].entries.count == 0)
// {
// //The cell is now empty. Give it back to the pool.
// var toRemove = cells.Elements[i];
// //There's no cleanup to do on the grid cell.
// //Its list is empty, and the rest is just value types.
// cells.RemoveAt(i);
// cellPool.GiveBack(toRemove);
// count--;
// }
// return;
// }
// //If it was not the correct index, let it continue searching.
// }
//}
////Getting here should be impossible.
}