internal override void OnUpdate(bool forceRebuild, HashSet <T> addedItems, HashSet <T> removedItems, HashSet <T> invalidItems)
{
if (forceRebuild)
{
// Total rebuild. Re-Add all Items.
_edges[0].Clear();
_edges[1].Clear();
_edges[2].Clear();
_itemInfos.Clear();
if (_broadPhase != null)
{
_broadPhase.Clear();
}
if (EnableSelfOverlaps)
{
SelfOverlaps.Clear();
}
foreach (var item in Items)
{
AddItem(item);
}
}
else
{
// Refit - the default case.
// First, remove all old items.
foreach (T removedItem in removedItems)
{
RemoveItem(removedItem);
}
// Second, update all invalid items before we add the new items.
if (invalidItems == null)
{
// Update all items.
foreach (ItemInfo itemInfo in _itemInfos)
{
UpdateItem(itemInfo);
}
}
else
{
// Update items marked as invalid.
foreach (T invalidItem in invalidItems)
{
UpdateItem(invalidItem);
}
}
// Third, add all the new items.
foreach (var addedItem in addedItems)
{
AddItem(addedItem);
}
}
// Update AABB of whole space.
UpdateAabb();
}
private void RemoveItem(T item)
{
// Remove broad phase info
var itemInfo = _itemInfos.Get(item);
if (itemInfo == null) // Abort if the object was not in the broad phase.
{
return;
}
// Remove from _itemInfos.
_itemInfos.Remove(item);
// Remove edges from edge lists.
for (int axisIndex = 0; axisIndex < 3; axisIndex++)
{
// Get indices in edge list.
int minEdgeIndex = itemInfo.MinEdgeIndices[axisIndex];
int maxEdgeIndex = itemInfo.MaxEdgeIndices[axisIndex];
// Update edge indices in itemInfo that point into edge list for edges above the min edge.
List <Edge> edgeList = _edges[axisIndex];
int numberOfEdges = edgeList.Count;
for (int i = minEdgeIndex + 1; i < numberOfEdges; i++)
{
// Subtract one for edges that were before the max edge to remove, and subtract 2 for the rest.
// TODO: We could split the loop into two parts. The part before maxEdgeIndex and the part after.
int decrement = (i > maxEdgeIndex) ? 2 : 1;
var currentItemInfo = edgeList[i].Info;
if (edgeList[i].IsMax == false)
{
currentItemInfo.MinEdgeIndices[axisIndex] = i - decrement;
}
else
{
currentItemInfo.MaxEdgeIndices[axisIndex] = i - decrement;
}
}
// Remove from edge list.
edgeList.RemoveAt(maxEdgeIndex);
edgeList.RemoveAt(minEdgeIndex);
}
// Remove related self-overlaps.
if (_broadPhase != null)
{
_broadPhase.Remove(item);
}
if (EnableSelfOverlaps)
{
// TODO: Lambda method produces garbage.
SelfOverlaps.RemoveWhere(overlap => Comparer.Equals(overlap.First, item) || Comparer.Equals(overlap.Second, item));
}
}
// Can only add overlapping pairs.
// Sort edge up and add overlapping pairs
private void SortMaxEdgeUp(List<Edge> edgeList, int axisIndex, int edgeIndex)
{
int maxIndex = edgeList.Count - 1;
if (edgeIndex == maxIndex)
return;
var edge = edgeList[edgeIndex];
var itemInfo = edge.Info;
int nextEdgeIndex = edgeIndex + 1;
var nextEdge = edgeList[nextEdgeIndex];
while (edge.Position >= nextEdge.Position)
{
var nextItemInfo = nextEdge.Info;
if (nextEdge.IsMax == false)
{
// New overlapping pair!
if (_broadPhase != null || EnableSelfOverlaps)
{
if (AreOverlapping(itemInfo, nextItemInfo, axisIndex))
{
var overlap = new Pair<T>(itemInfo.Item, nextItemInfo.Item);
if (Filter == null || Filter.Filter(overlap))
{
if (_broadPhase != null)
_broadPhase.Add(overlap);
if (EnableSelfOverlaps)
SelfOverlaps.Add(overlap);
}
}
}
nextItemInfo.MinEdgeIndices[axisIndex] = edgeIndex; // Min edge was swapped down.
}
else
{
nextItemInfo.MaxEdgeIndices[axisIndex] = edgeIndex; // Max edge was swapped down.
}
itemInfo.MaxEdgeIndices[axisIndex] = nextEdgeIndex; // Max edge was swapped up.
// Swap edges in edge list.
edgeList[edgeIndex] = nextEdge;
edgeList[nextEdgeIndex] = edge;
edgeIndex++;
if (edgeIndex == maxIndex)
return;
nextEdgeIndex++;
nextEdge = edgeList[nextEdgeIndex];
}
}
// Can only add overlapping pairs.
// Sort edge down and add overlapping pairs.
private void SortMinEdgeDown(List<Edge> edgeList, int axisIndex, int edgeIndex)
{
if (edgeIndex == 0)
return;
var edge = edgeList[edgeIndex];
var itemInfo = edge.Info;
int previousEdgeIndex = edgeIndex - 1;
Edge previousEdge = edgeList[previousEdgeIndex];
while (edge.Position <= previousEdge.Position)
{
var previousItemInfo = previousEdge.Info;
if (previousEdge.IsMax)
{
// New overlapping pair!
if (_broadPhase != null || EnableSelfOverlaps)
{
if (AreOverlapping(itemInfo, previousItemInfo, axisIndex))
{
var overlap = new Pair<T>(itemInfo.Item, previousItemInfo.Item);
if (Filter == null || Filter.Filter(overlap))
{
if (_broadPhase != null)
_broadPhase.Add(overlap);
if (EnableSelfOverlaps)
SelfOverlaps.Add(overlap);
}
}
}
previousItemInfo.MaxEdgeIndices[axisIndex] = edgeIndex; // Max edge was swapped up.
}
else
{
previousItemInfo.MinEdgeIndices[axisIndex] = edgeIndex; // Min edge was swapped up.
}
itemInfo.MinEdgeIndices[axisIndex] = previousEdgeIndex; // Min edge was swapped down.
// Swap edges in edge list.
edgeList[edgeIndex] = previousEdge;
edgeList[previousEdgeIndex] = edge;
edgeIndex--;
if (edgeIndex == 0)
return;
previousEdgeIndex--;
previousEdge = edgeList[previousEdgeIndex];
}
}
/// <inheritdoc/>
internal override void OnUpdate(bool forceRebuild, HashSet <T> addedItems, HashSet <T> removedItems, HashSet <T> invalidItems)
{
if (EnableSelfOverlaps)
{
// Need to find all self-overlaps.
SelfOverlaps.Clear();
// Test all items against all items.
HashSet <T> .Enumerator outerEnumerator = Items.GetEnumerator();
while (outerEnumerator.MoveNext())
{
T item = outerEnumerator.Current;
Aabb aabb = GetAabbForItem(item);
// Duplicate struct enumerator at current position.
HashSet <T> .Enumerator innerEnumerator = outerEnumerator;
while (innerEnumerator.MoveNext())
{
T otherItem = innerEnumerator.Current;
Aabb otherAabb = GetAabbForItem(otherItem);
Pair <T> overlap = new Pair <T>(item, otherItem);
if (Filter == null || Filter.Filter(overlap))
{
if (GeometryHelper.HaveContact(aabb, otherAabb))
{
SelfOverlaps.Add(overlap);
}
}
}
}
// If Items is a IList<T>, which has an indexer, we can use the following code.
//for (int i = 0; i < Items.Count; i++)
//{
// var itemI = Items[i];
// var aabbI = GetAabbForItem(itemI);
// for (int j = i + 1; j < Items.Count; j++)
// {
// var itemJ = Items[j];
// var aabbJ = GetAabbForItem(itemJ);
// var overlap = new Pair<T>(itemI, itemJ);
// if (Filter == null || Filter.Filter(overlap))
// if (GeometryHelper.HaveContact(aabbI, aabbJ))
// SelfOverlaps.Add(overlap);
// }
//}
}
}
/// <summary>
/// Recomputes the self-overlaps.
/// </summary>
private void UpdateSelfOverlaps()
{
if (!EnableSelfOverlaps)
{
return;
}
SelfOverlaps.Clear();
// Get intra-overlaps from static partition.
AddSelfOverlaps(StaticPartition.GetOverlaps());
// Get intra-overlaps from dynamic partition.
AddSelfOverlaps(DynamicPartition.GetOverlaps());
// Get inter-overlaps between static and dynamic partition.
AddSelfOverlaps(StaticPartition.GetOverlaps(DynamicPartition));
}
// Can only remove overlapping pairs.
// Sort edge down and remove pairs that are not overlapping anymore.
// Needs to called after SortMinEdgeDown.
private void SortMaxEdgeDown(List <Edge> edgeList, int axisIndex, int edgeIndex)
{
var edge = edgeList[edgeIndex];
var itemInfo = edge.Info;
int previousEdgeIndex = edgeIndex - 1;
var previousEdge = edgeList[previousEdgeIndex];
while (edge.Position < previousEdge.Position)
{
var previousItemInfo = previousEdge.Info;
if (previousEdge.IsMax == false)
{
// Remove overlapping pair.
var overlap = new Pair <T>(itemInfo.Item, previousItemInfo.Item);
if (_broadPhase != null)
{
_broadPhase.Remove(overlap);
}
if (EnableSelfOverlaps)
{
SelfOverlaps.Remove(overlap);
}
previousItemInfo.MinEdgeIndices[axisIndex] = edgeIndex; // Min edge was swapped up.
}
else
{
previousItemInfo.MaxEdgeIndices[axisIndex] = edgeIndex; // Max edge was swapped up.
}
itemInfo.MaxEdgeIndices[axisIndex] = previousEdgeIndex; // Max edge was swapped down.
// Swap edges in edge list.
edgeList[edgeIndex] = previousEdge;
edgeList[previousEdgeIndex] = edge;
edgeIndex--;
previousEdgeIndex--;
previousEdge = edgeList[previousEdgeIndex];
}
}
// Can only remove overlapping pairs.
// Sort edge up and remove pairs that are not overlapping anymore.
// Needs to be called after SortMaxEdgeUp.
private void SortMinEdgeUp(List <Edge> edgeList, int axisIndex, int edgeIndex)
{
var edge = edgeList[edgeIndex];
var itemInfo = edge.Info;
int nextEdgeIndex = edgeIndex + 1;
var nextEdge = edgeList[nextEdgeIndex];
while (edge.Position > nextEdge.Position)
{
var nextItemInfo = nextEdge.Info;
if (nextEdge.IsMax)
{
// Remove overlapping pair.
var overlap = new Pair <T>(itemInfo.Item, nextItemInfo.Item);
if (_broadPhase != null)
{
_broadPhase.Remove(overlap);
}
if (EnableSelfOverlaps)
{
SelfOverlaps.Remove(overlap);
}
nextItemInfo.MaxEdgeIndices[axisIndex] = edgeIndex; // Max edge was swapped down.
}
else
{
nextItemInfo.MinEdgeIndices[axisIndex] = edgeIndex; // Min edge was swapped down.
}
itemInfo.MinEdgeIndices[axisIndex] = nextEdgeIndex; // Min edge was swapped up.
// Swap edges in edge list.
edgeList[edgeIndex] = nextEdge;
edgeList[nextEdgeIndex] = edge;
edgeIndex++;
nextEdgeIndex++;
nextEdge = edgeList[nextEdgeIndex];
}
}
/// <summary>
/// Updates the self-overlaps.
/// </summary>
private void UpdateSelfOverlaps()
{
if (EnableSelfOverlaps)
{
// Recompute all self-overlaps by making a tree vs. tree test.
SelfOverlaps.Clear();
if (_root != null)
{
// Important: Do not call GetOverlaps(this) because this would lead to recursive
// Update() calls!
foreach (var overlap in GetOverlapsImpl(this))
{
Debug.Assert(FilterSelfOverlap(overlap), "Filtering should have been applied.");
SelfOverlaps.Add(overlap);
}
}
}
}
private void UpdateSelfOverlaps()
{
if (EnableSelfOverlaps)
{
// Update self-overlaps.
SelfOverlaps.Clear();
// ----- Compute self-overlaps using tree vs. tree test.
if (_root == null)
{
return;
}
// Important: Do not call GetOverlaps(this) because this would lead to recursive
// Update() calls!
foreach (var overlap in GetOverlapsImpl(this))
{
Debug.Assert(FilterSelfOverlap(overlap), "Filtering should have been applied.");
SelfOverlaps.Add(overlap);
}
// ----- Compute self-overlaps using leaf vs. tree test.
//if (_root != null)
//{
// foreach (var leave in _leaves)
// {
// foreach (var touchedItem in GetOverlaps(leave.Aabb))
// {
// var overlap = new Pair<T>(leave.Item, touchedItem);
// if (FilterSelfOverlap(overlap))
// SelfOverlaps.Add(overlap);
// }
// }
//}
}
}
private void UpdateSelfOverlaps(HashSet <T> addedItems, HashSet <T> removedItems, HashSet <T> invalidItems, List <Node> invalidNodes)
{
if (!EnableSelfOverlaps)
{
return;
}
// If the entire partition or a substantial amount of nodes is invalid use a tree vs. tree
// checks. (Faster than leaf vs. tree checks. However, we need to determine the exact
// threshold at which tree vs. tree is cheaper. The current threshold of Count / 2 is just
// a guess.)
if (invalidItems == null || addedItems.Count + invalidItems.Count > Count / 2)
{
// Recompute all self-overlaps by making a tree vs. tree test.
SelfOverlaps.Clear();
if (_root != null)
{
// Important: Do not call GetOverlaps(this) because this would lead to recursive
// Update() calls!
// Note: Filtering is applied in GetOverlapsImpl(this).
foreach (var overlap in GetOverlapsImpl(this))
{
SelfOverlaps.Add(overlap);
}
}
}
else
{
// Merge invalid and removed items into single set.
// Store result in invalidItems.
if (invalidItems.Count > 0 && removedItems.Count > 0)
{
// Merge smaller set into larger set.
if (invalidItems.Count < removedItems.Count)
{
MathHelper.Swap(ref invalidItems, ref removedItems);
}
foreach (var item in removedItems)
{
invalidItems.Add(item);
}
}
else if (removedItems.Count > 0)
{
invalidItems = removedItems;
}
// Remove invalid entries from self-overlaps.
if (invalidItems.Count > 0)
{
var invalidOverlaps = DigitalRune.ResourcePools <Pair <T> > .Lists.Obtain();
foreach (var overlap in SelfOverlaps)
{
if (invalidItems.Contains(overlap.First) || invalidItems.Contains(overlap.Second))
{
invalidOverlaps.Add(overlap);
}
}
foreach (var overlap in invalidOverlaps)
{
SelfOverlaps.Remove(overlap);
}
DigitalRune.ResourcePools <Pair <T> > .Lists.Recycle(invalidOverlaps);
}
// Compute new overlaps for all nodes that were updated in this frame.
if (invalidNodes != null)
{
foreach (var node in invalidNodes)
{
foreach (var touchedItem in GetOverlapsImpl(node))
{
var overlap = new Pair <T>(node.Item, touchedItem);
if (Filter == null || Filter.Filter(overlap))
{
SelfOverlaps.Add(overlap);
}
}
}
}
}
}
private void AddSelfOverlap(Pair <T> overlap)
{
Debug.Assert(FilterSelfOverlap(overlap), "Filtering should have been applied.");
SelfOverlaps.Add(overlap);
}
