private IEnumerable<Pair<T>> GetOverlapsImpl(Vector3 scale, ISpatialPartition<T> otherPartition, Vector3 otherScale, Pose otherPose)
{
// Compute transformations.
Vector3 scaleInverse = Vector3.One / scale;
Vector3 otherScaleInverse = Vector3.One / otherScale;
Pose toLocal = otherPose;
Pose toOther = otherPose.Inverse;
// Transform the AABB of the other partition into space of the this partition.
var otherAabb = otherPartition.Aabb;
otherAabb = otherAabb.GetAabb(otherScale, toLocal); // Apply local scale and transform to scaled local space of this partition.
otherAabb.Scale(scaleInverse); // Transform to unscaled local space of this partition.
var leafNodes = GetLeafNodes(otherAabb);
foreach (var leaf in leafNodes)
{
// Transform AABB of this partition into space of the other partition.
Aabb aabb = leaf.Aabb.GetAabb(scale, toOther); // Apply local scale and transform to scaled local space of other partition.
aabb.Scale(otherScaleInverse); // Transform to unscaled local space of other partition.
foreach (var otherCandidate in otherPartition.GetOverlaps(aabb))
{
var overlap = new Pair<T>(leaf.Item, otherCandidate);
if (Filter == null || Filter.Filter(overlap))
yield return overlap;
}
}
#else
// Avoiding garbage:
return GetOverlapsWithTransformedPartitionWork.Create(this, otherPartition, leafNodes, ref scale, ref otherScaleInverse, ref toOther);
}
private void TestGetOverlaps1(ISpatialPartition <int> partition)
{
// Temporarily add random test object.
var randomTestObject = new TestObject(GetRandomAabb());
_testObjects.Add(randomTestObject);
// Compute desired result.
var desiredResults = new List <int>();
foreach (var testObject in _testObjects)
{
if (testObject == randomTestObject)
{
continue;
}
if (partition.Filter == null || partition.Filter.Filter(new Pair <int>(randomTestObject.Id, testObject.Id)))
{
if (GeometryHelper.HaveContact(randomTestObject.Aabb, testObject.Aabb))
{
desiredResults.Add(testObject.Id);
}
}
}
var results = partition.GetOverlaps(randomTestObject.Id).ToList();
CompareResults(desiredResults, results, "GetOverlaps(T) returns different number of results.");
_testObjects.Remove(randomTestObject);
}
protected override bool OnNext(out Pair <T> current)
{
while (true)
{
if (_otherCandidates == null)
{
if (_leafNodes.MoveNext())
{
var leaf = _leafNodes.Current;
_otherCandidates = _otherPartition.GetOverlaps(leaf.Aabb).GetEnumerator();
}
else
{
current = default(Pair <T>);
return(false);
}
}
while (_otherCandidates.MoveNext())
{
var leaf = _leafNodes.Current;
var otherCandidate = _otherCandidates.Current;
var overlap = new Pair <T>(leaf.Item, otherCandidate);
if (_partition.Filter == null || _partition.Filter.Filter(overlap))
{
current = overlap;
return(true);
}
}
_otherCandidates.Dispose();
_otherCandidates = null;
}
}
/// <inheritdoc/>
public IEnumerable<Pair<int>> GetOverlaps(ISpatialPartition<int> otherPartition)
{
if (otherPartition == null)
throw new ArgumentNullException("otherPartition");
var otherBasePartition = otherPartition as BasePartition<int>;
if (otherBasePartition != null)
otherBasePartition.UpdateInternal();
else
otherPartition.Update(false);
Update(false);
#if !POOL_ENUMERABLES
// Test all leaf nodes that touch the other partition's AABB.
foreach (var leaf in GetLeafNodes(otherPartition.Aabb))
{
var otherCandidates = otherPartition.GetOverlaps(GetAabb(leaf));
// We return one pair for each candidate vs. otherItem overlap.
foreach (var otherCandidate in otherCandidates)
{
var overlap = new Pair<int>(leaf.Item, otherCandidate);
if (Filter == null || Filter.Filter(overlap))
yield return overlap;
}
}
#else
return GetOverlapsWithPartitionWork.Create(this, otherPartition);
#endif
}
/// <inheritdoc/>
public IEnumerable<Pair<int>> GetOverlaps(ISpatialPartition<int> otherPartition)
{
if (otherPartition == null)
throw new ArgumentNullException("otherPartition");
var otherBasePartition = otherPartition as BasePartition<int>;
if (otherBasePartition != null)
otherBasePartition.UpdateInternal();
else
otherPartition.Update(false);
Update(false);
// Test all leaf nodes that touch the other partition's AABB.
foreach (var leaf in GetLeafNodes(otherPartition.Aabb))
{
var otherCandidates = otherPartition.GetOverlaps(GetAabb(leaf));
// We return one pair for each candidate vs. otherItem overlap.
foreach (var otherCandidate in otherCandidates)
{
var overlap = new Pair<int>(leaf.Item, otherCandidate);
if (Filter == null || Filter.Filter(overlap))
yield return overlap;
}
}
#else
return GetOverlapsWithPartitionWork.Create(this, otherPartition);
}
/// <inheritdoc/>
public virtual IEnumerable <Pair <T> > GetOverlaps(Vector3F scale, Pose pose, ISpatialPartition <T> otherPartition, Vector3F otherScale, Pose otherPose)
{
if (otherPartition == null)
{
throw new ArgumentNullException("otherPartition");
}
var otherBasePartition = otherPartition as BasePartition <T>;
if (otherBasePartition != null)
{
otherBasePartition.UpdateInternal();
}
else
{
otherPartition.Update(false);
}
UpdateInternal();
// Compute transformations.
Vector3F scaleInverse = Vector3F.One / scale;
Vector3F otherScaleInverse = Vector3F.One / otherScale;
Pose toLocal = pose.Inverse * otherPose;
Pose toOther = toLocal.Inverse;
// Transform the AABB of the other partition into space of the this partition.
var otherAabb = otherPartition.Aabb;
otherAabb = otherAabb.GetAabb(otherScale, toLocal); // Apply local scale and transform to scaled local space of this partition.
otherAabb.Scale(scaleInverse); // Transform to unscaled local space of this partition.
var candidates = GetOverlaps(otherAabb);
#if !POOL_ENUMERABLES
foreach (var candidate in candidates)
{
// Transform AABB of this partition into space of the other partition.
var aabb = GetAabbForItem(candidate);
aabb = aabb.GetAabb(scale, toOther); // Apply local scale and transform to scaled local space of other partition.
aabb.Scale(otherScaleInverse); // Transform to unscaled local space of other partition.
foreach (var otherCandidate in otherPartition.GetOverlaps(aabb))
{
var overlap = new Pair <T>(candidate, otherCandidate);
if (Filter == null || Filter.Filter(overlap))
{
yield return(overlap);
}
}
}
#else
// Avoiding garbage:
return(GetOverlapsWithTransformedPartitionWork.Create(this, otherPartition, candidates, ref scale, ref otherScaleInverse, ref toOther));
#endif
}
/// <inheritdoc/>
public virtual IEnumerable <Pair <T> > GetOverlaps(ISpatialPartition <T> otherPartition)
{
if (otherPartition == null)
{
throw new ArgumentNullException("otherPartition");
}
var otherBasePartition = otherPartition as BasePartition <T>;
if (otherBasePartition != null)
{
otherBasePartition.UpdateInternal();
}
else
{
otherPartition.Update(false);
}
UpdateInternal();
#if !POOL_ENUMERABLES
// Get all items that touch the other partition's AABB.
var candidates = GetOverlaps(otherPartition.Aabb);
// Now, we test each candidate against the other partition.
foreach (var candidate in candidates)
{
Aabb candidateAabb = GetAabbForItem(candidate);
var otherCandidates = otherPartition.GetOverlaps(candidateAabb);
// We return one pair for each candidate vs. otherItem overlap.
foreach (var otherCandidate in otherCandidates)
{
var overlap = new Pair <T>(candidate, otherCandidate);
if (Filter == null || Filter.Filter(overlap))
{
yield return(overlap);
}
}
}
#else
// Avoiding garbage:
return(GetOverlapsWithPartitionWork.Create(this, otherPartition));
#endif
}
/// <inheritdoc/>
public IEnumerable<Pair<int>> GetOverlaps(Vector3 scale, Pose pose, ISpatialPartition<int> otherPartition, Vector3 otherScale, Pose otherPose)
{
if (otherPartition == null)
throw new ArgumentNullException("otherPartition");
var otherBasePartition = otherPartition as BasePartition<int>;
if (otherBasePartition != null)
otherBasePartition.UpdateInternal();
else
otherPartition.Update(false);
Update(false);
// Compute transformations.
Vector3 scaleInverse = Vector3.One / scale;
Vector3 otherScaleInverse = Vector3.One / otherScale;
Pose toLocal = pose.Inverse * otherPose;
Pose toOther = toLocal.Inverse;
// Transform the AABB of the other partition into space of the this partition.
var otherAabb = otherPartition.Aabb;
otherAabb = otherAabb.GetAabb(otherScale, toLocal); // Apply local scale and transform to scaled local space of this partition.
otherAabb.Scale(scaleInverse); // Transform to unscaled local space of this partition.
var leafNodes = GetLeafNodes(otherAabb);
foreach (var leaf in leafNodes)
{
// Transform AABB of this partition into space of the other partition.
var aabb = GetAabb(leaf);
aabb = aabb.GetAabb(scale, toOther); // Apply local scale and transform to scaled local space of other partition.
aabb.Scale(otherScaleInverse); // Transform to unscaled local space of other partition.
foreach (var otherCandidate in otherPartition.GetOverlaps(aabb))
{
var overlap = new Pair<int>(leaf.Item, otherCandidate);
if (Filter == null || Filter.Filter(overlap))
yield return overlap;
}
}
#else
return GetOverlapsWithTransformedPartitionWork.Create(this, otherPartition, leafNodes, ref scale, ref otherScaleInverse, ref toOther);
}
private void TestGetOverlaps0(ISpatialPartition <int> partition)
{
var aabb = GetRandomAabb();
// Compute desired result.
var desiredResults = new List <int>();
foreach (var testObject in _testObjects)
{
if (GeometryHelper.HaveContact(aabb, testObject.Aabb))
{
desiredResults.Add(testObject.Id);
}
}
var results = partition.GetOverlaps(aabb).ToList();
CompareResults(desiredResults, results, "GetOverlaps(Aabb) returns different number of results.");
}
private IEnumerable<Pair<T>> GetOverlapsImpl(ISpatialPartition<T> otherPartition)
{
// Test all leaf nodes that touch the other partition's AABB.
foreach (var leaf in GetLeafNodes(otherPartition.Aabb))
{
var otherCandidates = otherPartition.GetOverlaps(leaf.Aabb);
// We return one pair for each candidate vs. otherItem overlap.
foreach (var otherCandidate in otherCandidates)
{
var overlap = new Pair<T>(leaf.Item, otherCandidate);
if (Filter == null || Filter.Filter(overlap))
yield return overlap;
}
}
#else
// Avoiding garbage:
return GetOverlapsWithPartitionWork.Create(this, otherPartition);
}
private void TestGetOverlaps2(ISpatialPartition <int> partition)
{
var aabb = GetRandomAabb();
var ray = new Ray(aabb.Minimum, aabb.Extent.Normalized, aabb.Extent.Length);
ray.Direction = RandomHelper.Random.NextVector3(-1, 1).Normalized;
// Compute desired result.
var desiredResults = new List <int>();
foreach (var testObject in _testObjects)
{
if (GeometryHelper.HaveContact(testObject.Aabb, ray))
{
desiredResults.Add(testObject.Id);
}
}
var results = partition.GetOverlaps(ray).ToList();
CompareResults(desiredResults, results, "GetOverlaps(Ray) returns different number of results.");
}
private void TestGetOverlaps3(ISpatialPartition <int> partition)
{
if (!partition.EnableSelfOverlaps)
{
return;
}
// Compute desired result.
var desiredResults = new List <Pair <int> >();
for (int i = 0; i < _testObjects.Count; i++)
{
var a = _testObjects[i];
for (int j = i + 1; j < _testObjects.Count; j++)
{
var b = _testObjects[j];
if (a != b)
{
if (partition.Filter == null || partition.Filter.Filter(new Pair <int>(a.Id, b.Id)))
{
if (GeometryHelper.HaveContact(a.Aabb, b.Aabb))
{
desiredResults.Add(new Pair <int>(a.Id, b.Id));
}
}
}
}
}
var results = partition.GetOverlaps().ToList();
if (desiredResults.Count != results.Count)
{
var distinct = results.Except(desiredResults).ToList();
}
CompareResults(desiredResults, results, "GetOverlaps() returns different number of results.");
}
private void TestGetOverlaps4(ISpatialPartition <int> partition)
{
// Compute desired result.
var desiredResults = new List <Pair <int> >();
foreach (var a in _testObjects)
{
foreach (var b in _testObjectsOfPartition2)
{
if (partition.Filter == null || partition.Filter.Filter(new Pair <int>(a.Id, b.Id)))
{
if (GeometryHelper.HaveContact(a.Aabb, b.Aabb))
{
desiredResults.Add(new Pair <int>(a.Id, b.Id));
}
}
}
}
var results = partition.GetOverlaps(_partition2).ToList();
CompareResults(desiredResults, results, "GetOverlaps(Partition) returns different number of results.");
}
private void TestGetOverlaps5(ISpatialPartition <int> partition)
{
// Get random pose for _partition2
var pose = new Pose(GetRandomAabb().Center, RandomHelper.Random.NextQuaternion());
var scale = RandomHelper.Random.NextVector3(0.1f, 3f);
// Compute desired result.
var desiredResults = new List <Pair <int> >();
foreach (var a in _testObjects)
{
foreach (var b in _testObjectsOfPartition2)
{
if (partition.Filter == null || partition.Filter.Filter(new Pair <int>(a.Id, b.Id)))
{
var aabbB = b.Aabb;
aabbB.Scale(scale);
var boxB = aabbB.Extent;
var poseB = pose * new Pose(aabbB.Center);
if (GeometryHelper.HaveContact(a.Aabb, boxB, poseB, true))
{
desiredResults.Add(new Pair <int>(a.Id, b.Id));
}
}
}
}
var results = partition.GetOverlaps(Vector3.One, Pose.Identity, _partition2, scale, pose).ToList();
if (desiredResults.Count > results.Count)
{
Debugger.Break();
}
CompareResults(desiredResults, results, "GetOverlaps(Partition, Pose, Scale) returns a wrong number of results or has missed an overlap.");
}
protected override bool OnNext(out Pair <T> current)
{
while (true)
{
if (_otherCandidates == null)
{
if (_candidates.MoveNext())
{
var candidate = _candidates.Current;
var aabb = _partition.GetAabbForItem(candidate);
aabb = aabb.GetAabb(_scale, _toOther);
aabb.Scale(_otherScaleInverse);
_otherCandidates = _otherPartition.GetOverlaps(aabb).GetEnumerator();
}
else
{
current = default(Pair <T>);
return(false);
}
}
while (_otherCandidates.MoveNext())
{
var candidate = _candidates.Current;
var otherCandidate = _otherCandidates.Current;
var overlap = new Pair <T>(candidate, otherCandidate);
if (_partition.Filter == null || _partition.Filter.Filter(overlap))
{
current = overlap;
return(true);
}
}
_otherCandidates.Dispose();
_otherCandidates = null;
}
}
private void TestGetOverlaps1(ISpatialPartition<int> partition)
{
// Temporarily add random test object.
var randomTestObject = new TestObject(GetRandomAabb());
_testObjects.Add(randomTestObject);
// Compute desired result.
var desiredResults = new List<int>();
foreach (var testObject in _testObjects)
{
if (testObject == randomTestObject)
continue;
if (partition.Filter == null || partition.Filter.Filter(new Pair<int>(randomTestObject.Id, testObject.Id)))
if (GeometryHelper.HaveContact(randomTestObject.Aabb, testObject.Aabb))
desiredResults.Add(testObject.Id);
}
var results = partition.GetOverlaps(randomTestObject.Id).ToList();
CompareResults(desiredResults, results, "GetOverlaps(T) returns different number of results.");
_testObjects.Remove(randomTestObject);
}
private void TestPartition(ISpatialPartition<int> partition)
{
partition.Clear();
Assert.AreEqual(0, partition.Count);
partition.EnableSelfOverlaps = true;
Assert.AreEqual(0, partition.GetOverlaps().Count());
Assert.AreEqual(0, partition.GetOverlaps(0).Count());
Assert.AreEqual(0, partition.GetOverlaps(new Aabb()).Count());
Assert.AreEqual(0, partition.GetOverlaps(_partition2).Count());
Assert.AreEqual(0, partition.GetOverlaps(Vector3F.One, Pose.Identity, _partition2, Vector3F.One, Pose.Identity).Count());
var testObject = new TestObject(new Aabb(new Vector3F(10), new Vector3F(10)));
_testObjects.Add(testObject);
partition.Add(testObject.Id);
for (int i = 0; i < 1000; i++)
{
// ----- Tests
Assert.AreEqual(_testObjects.Count, partition.Count, "Wrong number of items.");
if (i > 10 && i % 6 == 0)
TestGetOverlaps0(partition);
if (i > 10 && i % 6 == 1)
TestGetOverlaps1(partition);
if (i > 10 && i % 6 == 2)
TestGetOverlaps2(partition);
if (i > 10 && i % 6 == 3)
TestGetOverlaps3(partition);
if (i > 10 && i % 6 == 4)
TestGetOverlaps4(partition);
if (i > 10 && i % 6 == 5)
TestGetOverlaps5(partition);
// Update partition. From time to time rebuild all.
// For the above tests update should have been called automatically!
partition.Update(i % 10 == 9);
TestAabb(partition);
var dice100 = RandomHelper.Random.Next(0, 100);
if (dice100 < 2)
{
// Test remove/re-add without Update inbetween.
if (partition.Count > 0)
{
partition.Remove(_testObjects[0].Id);
partition.Add(_testObjects[0].Id);
}
}
dice100 = RandomHelper.Random.Next(0, 100);
if (dice100 < 10)
{
// Remove objects.
int removeCount = RandomHelper.Random.NextInteger(1, 4);
for (int k = 0; k < removeCount && partition.Count > 0; k++)
{
var index = RandomHelper.Random.NextInteger(0, partition.Count - 1);
var obj = _testObjects[index];
_testObjects.Remove(obj);
partition.Remove(obj.Id);
}
}
dice100 = RandomHelper.Random.Next(0, 100);
if (dice100 < 10)
{
// Add new objects.
int addCount = RandomHelper.Random.NextInteger(1, 4);
for (int k = 0; k < addCount; k++)
{
var newObj = new TestObject(GetRandomAabb());
_testObjects.Add(newObj);
partition.Add(newObj.Id);
}
}
else
{
// Move an object.
int moveCount = RandomHelper.Random.NextInteger(1, 10);
for (int k = 0; k < moveCount && partition.Count > 0; k++)
{
var index = RandomHelper.Random.NextInteger(0, partition.Count - 1);
var obj = _testObjects[index];
obj.Aabb = GetRandomAabb();
partition.Invalidate(obj.Id);
}
}
// From time to time invalidate all.
if (dice100 < 3)
partition.Invalidate();
// From time to time change EnableSelfOverlaps.
if (dice100 > 3 && dice100 < 6)
partition.EnableSelfOverlaps = false;
else if (dice100 < 10)
partition.EnableSelfOverlaps = true;
// From time to time change filter.
if (dice100 > 10 && dice100 < 13)
{
partition.Filter = null;
}
else if (dice100 < 10)
{
if (partition.Filter == null)
partition.Filter = new DelegatePairFilter<int>(AreInSameGroup);
}
}
partition.Clear();
Assert.AreEqual(0, partition.Count);
}
private void TestGetOverlaps2(ISpatialPartition<int> partition)
{
var aabb = GetRandomAabb();
var ray = new Ray(aabb.Minimum, aabb.Extent.Normalized, aabb.Extent.Length);
ray.Direction = RandomHelper.Random.NextVector3F(-1, 1).Normalized;
// Compute desired result.
var desiredResults = new List<int>();
foreach (var testObject in _testObjects)
{
if (GeometryHelper.HaveContact(testObject.Aabb, ray))
desiredResults.Add(testObject.Id);
}
var results = partition.GetOverlaps(ray).ToList();
CompareResults(desiredResults, results, "GetOverlaps(Ray) returns different number of results.");
}
private void TestGetOverlaps3(ISpatialPartition<int> partition)
{
if (!partition.EnableSelfOverlaps)
return;
// Compute desired result.
var desiredResults = new List<Pair<int>>();
for (int i = 0; i < _testObjects.Count; i++)
{
var a = _testObjects[i];
for (int j = i + 1; j < _testObjects.Count; j++)
{
var b = _testObjects[j];
if (a != b)
if (partition.Filter == null || partition.Filter.Filter(new Pair<int>(a.Id, b.Id)))
if (GeometryHelper.HaveContact(a.Aabb, b.Aabb))
desiredResults.Add(new Pair<int>(a.Id, b.Id));
}
}
var results = partition.GetOverlaps().ToList();
if (desiredResults.Count != results.Count)
{
var distinct = results.Except(desiredResults).ToList();
}
CompareResults(desiredResults, results, "GetOverlaps() returns different number of results.");
}
/// <inheritdoc/>
public IEnumerable<Pair<int>> GetOverlaps(Vector3F scale, Pose pose, ISpatialPartition<int> otherPartition, Vector3F otherScale, Pose otherPose)
{
if (otherPartition == null)
throw new ArgumentNullException("otherPartition");
var otherBasePartition = otherPartition as BasePartition<int>;
if (otherBasePartition != null)
otherBasePartition.UpdateInternal();
else
otherPartition.Update(false);
Update(false);
// Compute transformations.
Vector3F scaleInverse = Vector3F.One / scale;
Vector3F otherScaleInverse = Vector3F.One / otherScale;
Pose toLocal = pose.Inverse * otherPose;
Pose toOther = toLocal.Inverse;
// Transform the AABB of the other partition into space of the this partition.
var otherAabb = otherPartition.Aabb;
otherAabb = otherAabb.GetAabb(otherScale, toLocal); // Apply local scale and transform to scaled local space of this partition.
otherAabb.Scale(scaleInverse); // Transform to unscaled local space of this partition.
var leafNodes = GetLeafNodes(otherAabb);
#if !POOL_ENUMERABLES
foreach (var leaf in leafNodes)
{
// Transform AABB of this partition into space of the other partition.
var aabb = GetAabb(leaf);
aabb = aabb.GetAabb(scale, toOther); // Apply local scale and transform to scaled local space of other partition.
aabb.Scale(otherScaleInverse); // Transform to unscaled local space of other partition.
foreach (var otherCandidate in otherPartition.GetOverlaps(aabb))
{
var overlap = new Pair<int>(leaf.Item, otherCandidate);
if (Filter == null || Filter.Filter(overlap))
yield return overlap;
}
}
#else
return GetOverlapsWithTransformedPartitionWork.Create(this, otherPartition, leafNodes, ref scale, ref otherScaleInverse, ref toOther);
#endif
}
private void TestGetOverlaps4(ISpatialPartition<int> partition)
{
// Compute desired result.
var desiredResults = new List<Pair<int>>();
foreach (var a in _testObjects)
{
foreach (var b in _testObjectsOfPartition2)
{
if (partition.Filter == null || partition.Filter.Filter(new Pair<int>(a.Id, b.Id)))
if (GeometryHelper.HaveContact(a.Aabb, b.Aabb))
desiredResults.Add(new Pair<int>(a.Id, b.Id));
}
}
var results = partition.GetOverlaps(_partition2).ToList();
CompareResults(desiredResults, results, "GetOverlaps(Partition) returns different number of results.");
}
private void TestGetOverlaps0(ISpatialPartition<int> partition)
{
var aabb = GetRandomAabb();
// Compute desired result.
var desiredResults = new List<int>();
foreach (var testObject in _testObjects)
{
if (GeometryHelper.HaveContact(aabb, testObject.Aabb))
desiredResults.Add(testObject.Id);
}
var results = partition.GetOverlaps(aabb).ToList();
CompareResults(desiredResults, results, "GetOverlaps(Aabb) returns different number of results.");
}
private void TestPartition(ISpatialPartition <int> partition)
{
partition.Clear();
Assert.AreEqual(0, partition.Count);
partition.EnableSelfOverlaps = true;
Assert.AreEqual(0, partition.GetOverlaps().Count());
Assert.AreEqual(0, partition.GetOverlaps(0).Count());
Assert.AreEqual(0, partition.GetOverlaps(new Aabb()).Count());
Assert.AreEqual(0, partition.GetOverlaps(_partition2).Count());
Assert.AreEqual(0, partition.GetOverlaps(Vector3.One, Pose.Identity, _partition2, Vector3.One, Pose.Identity).Count());
var testObject = new TestObject(new Aabb(new Vector3(10), new Vector3(10)));
_testObjects.Add(testObject);
partition.Add(testObject.Id);
for (int i = 0; i < 1000; i++)
{
// ----- Tests
Assert.AreEqual(_testObjects.Count, partition.Count, "Wrong number of items.");
if (i > 10 && i % 6 == 0)
{
TestGetOverlaps0(partition);
}
if (i > 10 && i % 6 == 1)
{
TestGetOverlaps1(partition);
}
if (i > 10 && i % 6 == 2)
{
TestGetOverlaps2(partition);
}
if (i > 10 && i % 6 == 3)
{
TestGetOverlaps3(partition);
}
if (i > 10 && i % 6 == 4)
{
TestGetOverlaps4(partition);
}
if (i > 10 && i % 6 == 5)
{
TestGetOverlaps5(partition);
}
// Update partition. From time to time rebuild all.
// For the above tests update should have been called automatically!
partition.Update(i % 10 == 9);
TestAabb(partition);
var dice100 = RandomHelper.Random.Next(0, 100);
if (dice100 < 2)
{
// Test remove/re-add without Update inbetween.
if (partition.Count > 0)
{
partition.Remove(_testObjects[0].Id);
partition.Add(_testObjects[0].Id);
}
}
dice100 = RandomHelper.Random.Next(0, 100);
if (dice100 < 10)
{
// Remove objects.
int removeCount = RandomHelper.Random.NextInteger(1, 4);
for (int k = 0; k < removeCount && partition.Count > 0; k++)
{
var index = RandomHelper.Random.NextInteger(0, partition.Count - 1);
var obj = _testObjects[index];
_testObjects.Remove(obj);
partition.Remove(obj.Id);
}
}
dice100 = RandomHelper.Random.Next(0, 100);
if (dice100 < 10)
{
// Add new objects.
int addCount = RandomHelper.Random.NextInteger(1, 4);
for (int k = 0; k < addCount; k++)
{
var newObj = new TestObject(GetRandomAabb());
_testObjects.Add(newObj);
partition.Add(newObj.Id);
}
}
else
{
// Move an object.
int moveCount = RandomHelper.Random.NextInteger(1, 10);
for (int k = 0; k < moveCount && partition.Count > 0; k++)
{
var index = RandomHelper.Random.NextInteger(0, partition.Count - 1);
var obj = _testObjects[index];
obj.Aabb = GetRandomAabb();
partition.Invalidate(obj.Id);
}
}
// From time to time invalidate all.
if (dice100 < 3)
{
partition.Invalidate();
}
// From time to time change EnableSelfOverlaps.
if (dice100 > 3 && dice100 < 6)
{
partition.EnableSelfOverlaps = false;
}
else if (dice100 < 10)
{
partition.EnableSelfOverlaps = true;
}
// From time to time change filter.
if (dice100 > 10 && dice100 < 13)
{
partition.Filter = null;
}
else if (dice100 < 10)
{
if (partition.Filter == null)
{
partition.Filter = new DelegatePairFilter <int>(AreInSameGroup);
}
}
}
partition.Clear();
Assert.AreEqual(0, partition.Count);
}
public override void Update(GameTime gameTime)
{
// If an item in a spatial partition changes (e.g. if it moves or if the AABB changes) you
// have to call
//_spatialPartition.Invalidate(changedGeometryobject);
// If many or all items have changed you can call
//_spatialPartition.Invalidate();
// After an item was invalidated, the spatial partition needs to be rebuilt. This happens
// automatically when needed or when you call
//_spatialPartition.Update(forceRebuild: false);
// Get a ray which shoots forward.
var cameraPose = GraphicsScreen.CameraNode.PoseWorld;
var ray = new Ray
{
Origin = cameraPose.Position,
Direction = cameraPose.ToWorldDirection(Vector3.Forward),
Length = 100,
};
// Draw objects.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
foreach (var geometricObject in _spatialPartition)
{
debugRenderer.DrawObject(geometricObject, Color.LightGreen, false, false);
}
GeometricObject closestHitGeometricObject = null;
Triangle closestHitTriangle = new Triangle();
float closestHitDistance = float.PositiveInfinity;
// Use the spatial partition to get all objects where the AABB overlaps the ray.
foreach (var geometricObject in _spatialPartition.GetOverlaps(ray))
{
var triangleMeshShape = (TriangleMeshShape)geometricObject.Shape;
// Transform the ray into the local space of the triangle mesh.
var localRay = new Ray
{
Origin = geometricObject.Pose.ToLocalPosition(ray.Origin),
Direction = geometricObject.Pose.ToLocalDirection(ray.Direction),
Length = ray.Length,
};
// Use the spatial partition of the mesh shape to compute all triangles where the
// AABB overlaps the ray.
foreach (var triangleIndex in triangleMeshShape.Partition.GetOverlaps(localRay))
{
var triangle = triangleMeshShape.Mesh.GetTriangle(triangleIndex);
// Check if ray intersects the triangle and remember the closest hit.
float hitDistance;
if (GeometryHelper.GetContact(localRay, triangle, false, out hitDistance) &&
hitDistance < closestHitDistance)
{
closestHitGeometricObject = geometricObject;
closestHitTriangle = triangle;
closestHitDistance = hitDistance;
}
}
}
// Draw hit triangle.
if (closestHitGeometricObject != null)
{
debugRenderer.DrawTriangle(closestHitTriangle, closestHitGeometricObject.Pose, Vector3.One, Color.Red, true, true);
}
}
private void TestGetOverlaps5(ISpatialPartition<int> partition)
{
// Get random pose for _partition2
var pose = new Pose(GetRandomAabb().Center, RandomHelper.Random.NextQuaternionF());
var scale = RandomHelper.Random.NextVector3F(0.1f, 3f);
// Compute desired result.
var desiredResults = new List<Pair<int>>();
foreach (var a in _testObjects)
{
foreach (var b in _testObjectsOfPartition2)
{
if (partition.Filter == null || partition.Filter.Filter(new Pair<int>(a.Id, b.Id)))
{
var aabbB = b.Aabb;
aabbB.Scale(scale);
var boxB = aabbB.Extent;
var poseB = pose * new Pose(aabbB.Center);
if (GeometryHelper.HaveContact(a.Aabb, boxB, poseB, true))
desiredResults.Add(new Pair<int>(a.Id, b.Id));
}
}
}
var results = partition.GetOverlaps(Vector3F.One, Pose.Identity, _partition2, scale, pose).ToList();
if (desiredResults.Count > results.Count)
Debugger.Break();
CompareResults(desiredResults, results, "GetOverlaps(Partition, Pose, Scale) returns a wrong number of results or has missed an overlap.");
}