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);
}
/// <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
}
private void CloneMeshWithPartition(ISpatialPartition <int> partition)
{
TriangleMeshShape meshShape = (TriangleMeshShape)_meshShape.Clone();
meshShape.Partition = partition;
TriangleMeshShape clone = meshShape.Clone() as TriangleMeshShape;
Assert.IsNotNull(clone);
Assert.IsNotNull(clone.Mesh);
Assert.IsTrue(clone.Mesh is TriangleMesh);
Assert.AreSame(_mesh, clone.Mesh);
Assert.AreEqual(_mesh.NumberOfTriangles, clone.Mesh.NumberOfTriangles);
for (int i = 0; i < _mesh.NumberOfTriangles; i++)
{
Triangle t = _mesh.GetTriangle(i);
Triangle tCloned = clone.Mesh.GetTriangle(i);
Assert.AreEqual(t.Vertex0, tCloned.Vertex0);
Assert.AreEqual(t.Vertex1, tCloned.Vertex1);
Assert.AreEqual(t.Vertex2, tCloned.Vertex2);
}
Assert.IsNotNull(clone.Partition);
Assert.IsInstanceOf(partition.GetType(), clone.Partition);
Assert.AreEqual(_mesh.NumberOfTriangles, clone.Partition.Count);
Assert.AreNotSame(partition, clone.Partition);
}
/// <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);
}
// TODO: Add DynamicAabbTree<T> vs. AabbTree<T>.
/// <inheritdoc/>
public override 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 (_root == null)
return LinqHelper.Empty<Pair<T>>();
var otherTree = otherPartition as DynamicAabbTree<T>;
if (otherTree == null)
{
// DynamicAabbTree<T> vs. ISpatialPartition<T>.
return GetOverlapsImpl(otherPartition);
}
else
{
// DynamicAabbTree<T> vs. DynamicAabbTree<T>
if (otherTree._root == null)
return LinqHelper.Empty<Pair<T>>();
return GetOverlapsImpl(otherTree);
}
}
public void GetClosestPointCandidates(Vector3F scale, Pose pose, ISpatialPartition<int> otherPartition, Vector3F otherScale, Pose otherPose, Func<int, int, float> callback)
{
if (otherPartition == null)
throw new ArgumentNullException("otherPartition");
if (callback == null)
throw new ArgumentNullException("callback");
// Make sure we are up-to-date.
var otherBasePartition = otherPartition as BasePartition<int>;
if (otherBasePartition != null)
otherBasePartition.UpdateInternal();
else
otherPartition.Update(false);
Update(false);
if (_numberOfItems == 0)
return;
if (otherPartition is ISupportClosestPointQueries<int>)
{
// ----- CompressedAabbTree vs. ISupportClosestPointQueries<int>
GetClosestPointCandidatesImpl(scale, pose, (ISupportClosestPointQueries<int>)otherPartition, otherScale, otherPose, callback);
}
else
{
// ----- CompressedAabbTree vs. *
GetClosestPointCandidatesImpl(otherPartition, callback);
}
}
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 TestAabb(ISpatialPartition <int> partition)
{
if (_testObjects.Count == 0)
{
return;
}
// Compute desired result.
var desiredAabb = _testObjects[0].Aabb;
_testObjects.ForEach(obj => desiredAabb.Grow(obj.Aabb));
// The AABB of the spatial partition can be slightly bigger.
// E.g. the CompressedAabbTree adds a margin to avoid divisions by zero.
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Minimum.X));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Minimum.Y));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Minimum.Z));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Maximum.X));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Maximum.Y));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Maximum.Z));
if (_conservativeAabb)
{
// AABB can be bigger than actual objects.
Assert.IsTrue(partition.Aabb.Contains(desiredAabb), "Wrong AABB: AABB is too small.");
}
else
{
// The AABB should be identical.
Assert.IsTrue(Vector3.AreNumericallyEqual(desiredAabb.Minimum, partition.Aabb.Minimum));
Assert.IsTrue(Vector3.AreNumericallyEqual(desiredAabb.Maximum, partition.Aabb.Maximum));
}
}
/// <summary>
/// Sets the spatial partition. (For use by the content pipeline only.)
/// </summary>
/// <param name="partition">The spatial partition.</param>
/// <remarks>
/// This method is used internally to directly set the spatial partition. The spatial partition
/// might already be initialized and should not be invalidated.
/// </remarks>
internal void SetPartition(ISpatialPartition <int> partition)
{
if (partition != null)
{
_partition = partition;
_partition.GetAabbForItem = i => Children[i].Aabb;
// ----- Validate spatial partition.
// Some spatial partitions, such as the CompressedAabbTree, are pre-initialized when
// loaded via content pipeline. Other spatial partitions need to be initialized manually.
int numberOfChildren = Children.Count;
if (_partition.Count != numberOfChildren)
{
// The partition is not initialized.
_partition.Clear();
for (int i = 0; i < numberOfChildren; i++)
{
_partition.Add(i);
}
_partition.Update(false);
}
else
{
// The partition is already initialized.
Debug.Assert(Enumerable.Range(0, numberOfChildren).All(_partition.Contains), "Invalid partition. The pre-initialized partition does not contain the same children as the CompositeShape.");
}
}
}
/// <inheritdoc/>
public override IEnumerable<Pair<T>> GetOverlaps(Vector3 scale, Pose pose, ISpatialPartition<T> otherPartition, Vector3 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();
if (_root == null)
return LinqHelper.Empty<Pair<T>>();
var otherTree = otherPartition as AdaptiveAabbTree<T>;
if (otherTree == null)
{
// AdaptiveAabbTree<T> vs. ISpatialPartition<T>.
return GetOverlapsImpl(scale, otherPartition, otherScale, pose.Inverse * otherPose);
}
else
{
// AdaptiveAabbTree<T> vs. AdaptiveAabbTree<T>
if (otherTree._root == null)
return LinqHelper.Empty<Pair<T>>();
return GetOverlapsImpl(scale, otherTree, otherScale, pose.Inverse * otherPose);
}
}
public void GetClosestPointCandidates(Vector3 scale, Pose pose, ISpatialPartition<int> otherPartition, Vector3 otherScale, Pose otherPose, Func<int, int, float> callback)
{
if (otherPartition == null)
throw new ArgumentNullException("otherPartition");
if (callback == null)
throw new ArgumentNullException("callback");
// Make sure we are up-to-date.
var otherBasePartition = otherPartition as BasePartition<int>;
if (otherBasePartition != null)
otherBasePartition.UpdateInternal();
else
otherPartition.Update(false);
Update(false);
if (_numberOfItems == 0)
return;
if (otherPartition is ISupportClosestPointQueries<int>)
{
// ----- CompressedAabbTree vs. ISupportClosestPointQueries<int>
GetClosestPointCandidatesImpl(scale, pose, (ISupportClosestPointQueries<int>)otherPartition, otherScale, otherPose, callback);
}
else
{
// ----- CompressedAabbTree vs. *
GetClosestPointCandidatesImpl(otherPartition, callback);
}
}
private void Start()
{
#if STUPID
using (new KristerTimer(
$"Blue-Noise Generator (Shitty Version, {numObjects} objects)",
1))
{
spatialPartition =
new StupidVersion(prefab,
transform,
SqRectilinear,
numObjects);
#elif KDTREE
using (new KristerTimer($"Blue-Noise Generator (k-d Tree Version, {numObjects} objects)", 1))
{
spatialPartition =
new KdTree(prefab, transform, SqEuclidean, numObjects);
#elif OCTREE
using (new KristerTimer($"Blue-Noise Generator (Octree Version, {numObjects} objects)", 1))
{
spatialPartition = new Octree(prefab, transform, numObjects);
#elif OFFSETOCTREE
using (new KristerTimer($"Blue-Noise Generator (Offset Octree Version, {numObjects} objects)", 1))
{
spatialPartition =
new OffsetOctree(prefab, transform, numObjects);
#endif
spatialPartition.Insert(Vector3.zero);
for (int pointIndex = 1;
pointIndex < numObjects;
pointIndex++)
{
var bestSqDistance = float.MinValue;
Vector3 bestCandidate = default;
for (int candidateIndex = 0;
candidateIndex < (pointIndex * sampleMultiplier) + 1;
candidateIndex++)
{
var candidate = GenerateRandomPoint();
float sqDistance = float.MinValue;
spatialPartition.FindNearestPoint(candidate,
out GameObject ignored,
out sqDistance);
if (sqDistance > bestSqDistance)
{
bestCandidate = candidate;
bestSqDistance = sqDistance;
}
}
spatialPartition.Insert(bestCandidate);
}
}
}
public static IEnumerable <Pair <T> > Create(AabbTree <T> partition, ISpatialPartition <T> otherPartition)
{
var enumerable = Pool.Obtain();
enumerable._partition = partition;
enumerable._otherPartition = otherPartition;
enumerable._leafNodes = partition.GetLeafNodes(otherPartition.Aabb).GetEnumerator();
return(enumerable);
}
public SpatialPartitionSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.Gray;
GraphicsScreen.DrawReticle = true;
SetCamera(new Vector3(0, 1, 10), 0, 0);
// Create a spatial partition. DigitalRune Geometry supports several types, see also
// http://digitalrune.github.io/DigitalRune-Documentation/html/e32cab3b-cc7c-42ee-8ec9-23dd4467edd0.htm#WhichPartition
// An AabbTree is useful for static objects. A DynamicAabbTree is good for moving objects.
// The spatial partition can manage different types of items. In this case it manages
// GeometricObjects. A delegate has to inform the spatial partition how to get the AABB
// of an object.
//_spatialPartition = new DynamicAabbTree<GeometricObject>
_spatialPartition = new AabbTree <GeometricObject>
{
GetAabbForItem = geometricObject => geometricObject.Aabb,
// Optional: The tree is automatically built using a mixed top-down/bottom-up approach.
// Bottom-up building is slower but produces better trees. If the tree building takes too
// long, we can lower the BottomUpBuildThreshold (default is 128).
//BottomUpBuildThreshold = 0,
// Optional: A filter can be set to disable certain kind of overlaps.
//Filter = ...
};
// Create a triangle mesh.
var triangleMesh = new SphereShape(1).GetMesh(0.01f, 4);
var triangleMeshShape = new TriangleMeshShape(triangleMesh)
{
// TriangleMeshShapes can also use a spatial partition to manage triangle.
// The items in the spatial partition are the triangle indices. The GetAabbForItem
// delegate is set automatically.
Partition = new AabbTree <int>(),
};
// Spatial partitions are built automatically when needed. However, it is still recommended
// to call Update to initialize the spatial partition explicitly.
triangleMeshShape.Partition.Update(false);
// Add a lot of triangle mesh objects to _spatialPartition.
var random = new Random();
for (int i = 0; i < 50; i++)
{
var randomPosition = new Vector3(random.NextFloat(-6, 6), random.NextFloat(-3, 3), random.NextFloat(-10, 0));
var geometricObject = new GeometricObject(triangleMeshShape, new Pose(randomPosition));
_spatialPartition.Add(geometricObject);
}
_spatialPartition.Update(false);
}
/// <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
}
protected override void Write(ContentWriter output, DualPartition <T> value)
{
dynamic internals = value.Internals;
ISpatialPartition <T> staticPartition = internals.StaticPartition;
ISpatialPartition <T> dynamicPartition = internals.DynamicPartition;
output.Write(value.EnableSelfOverlaps);
output.WriteSharedResource(value.Filter);
output.WriteObject(staticPartition);
output.WriteObject(dynamicPartition);
}
public SpatialPartitionSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.Gray;
GraphicsScreen.DrawReticle = true;
SetCamera(new Vector3F(0, 1, 10), 0, 0);
// Create a spatial partition. DigitalRune Geometry supports several types, see also
// http://digitalrune.github.io/DigitalRune-Documentation/html/e32cab3b-cc7c-42ee-8ec9-23dd4467edd0.htm#WhichPartition
// An AabbTree is useful for static objects. A DynamicAabbTree is good for moving objects.
// The spatial partition can manage different types of items. In this case it manages
// GeometricObjects. A delegate has to inform the spatial partition how to get the AABB
// of an object.
//_spatialPartition = new DynamicAabbTree<GeometricObject>
_spatialPartition = new AabbTree<GeometricObject>
{
GetAabbForItem = geometricObject => geometricObject.Aabb,
// Optional: The tree is automatically built using a mixed top-down/bottom-up approach.
// Bottom-up building is slower but produces better trees. If the tree building takes too
// long, we can lower the BottomUpBuildThreshold (default is 128).
//BottomUpBuildThreshold = 0,
// Optional: A filter can be set to disable certain kind of overlaps.
//Filter = ...
};
// Create a triangle mesh.
var triangleMesh = new SphereShape(1).GetMesh(0.01f, 4);
var triangleMeshShape = new TriangleMeshShape(triangleMesh)
{
// TriangleMeshShapes can also use a spatial partition to manage triangle.
// The items in the spatial partition are the triangle indices. The GetAabbForItem
// delegate is set automatically.
Partition = new AabbTree<int>(),
};
// Spatial partitions are built automatically when needed. However, it is still recommended
// to call Update to initialize the spatial partition explicitly.
triangleMeshShape.Partition.Update(false);
// Add a lot of triangle mesh objects to _spatialPartition.
var random = new Random();
for (int i = 0; i < 50; i++)
{
var randomPosition = new Vector3F(random.NextFloat(-6, 6), random.NextFloat(-3, 3), random.NextFloat(-10, 0));
var geometricObject = new GeometricObject(triangleMeshShape, new Pose(randomPosition));
_spatialPartition.Add(geometricObject);
}
_spatialPartition.Update(false);
}
/// <inheritdoc/>
public override IEnumerable<Pair<T>> GetOverlaps(ISpatialPartition<T> otherPartition)
{
UpdateInternal();
var overlapsStatic = StaticPartition.GetOverlaps(otherPartition);
var overlapsDynamic = DynamicPartition.GetOverlaps(otherPartition);
return overlapsStatic.Concat(overlapsDynamic);
#else
return ConcatWork<Pair<T>>.Create(overlapsStatic, overlapsDynamic);
}
/// <inheritdoc/>
public override IEnumerable<Pair<T>> GetOverlaps(Vector3 scale, Pose pose, ISpatialPartition<T> otherPartition, Vector3 otherScale, Pose otherPose)
{
UpdateInternal();
var overlapsStatic = StaticPartition.GetOverlaps(scale, pose, otherPartition, otherScale, otherPose);
var overlapsDynamic = DynamicPartition.GetOverlaps(scale, pose, otherPartition, otherScale, otherPose);
return overlapsStatic.Concat(overlapsDynamic);
#else
return ConcatWork<Pair<T>>.Create(overlapsStatic, overlapsDynamic);
}
/// <summary>
/// Initializes a new instance of the <see cref="CollisionDetectionBroadPhase"/> class.
/// </summary>
public CollisionDetectionBroadPhase(CollisionDomain collisionDomain)
{
_collisionDomain = collisionDomain;
// Register event handler.
_collisionDomain.CollisionObjects.CollectionChanged += OnCollisionObjectsChanged;
// Per default we use Sweep and Prune.
SpatialPartition = new SweepAndPruneSpace <CollisionObject>();
CandidatePairs = new ContactSetCollection();
}
/// <summary>
/// Initializes a new instance of the <see cref="TriangleMeshShape"/> class from the given
/// triangle mesh.
/// </summary>
/// <param name="mesh">The mesh.</param>
/// <param name="enableContactWelding">
/// If set to <see langword="true"/> contact welding is enabled; otherwise, the shape will not
/// use contact welding. See <see cref="EnableContactWelding"/> for more information.
/// </param>
/// <param name="partition">
/// The spatial partition (see <see cref="Partition"/>). Can be <see langword="null"/> if no
/// partition should be used.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="mesh"/> is <see langword="null"/>.
/// </exception>
public TriangleMeshShape(ITriangleMesh mesh, bool enableContactWelding, ISpatialPartition <int> partition)
{
if (mesh == null)
{
throw new ArgumentNullException("mesh");
}
_mesh = mesh;
Partition = partition;
EnableContactWelding = enableContactWelding;
}
/// <inheritdoc/>
public override IEnumerable <Pair <T> > GetOverlaps(ISpatialPartition <T> otherPartition)
{
UpdateInternal();
var overlapsStatic = StaticPartition.GetOverlaps(otherPartition);
var overlapsDynamic = DynamicPartition.GetOverlaps(otherPartition);
#if !POOL_ENUMERABLES
return(overlapsStatic.Concat(overlapsDynamic));
#else
return(ConcatWork <Pair <T> > .Create(overlapsStatic, overlapsDynamic));
#endif
}
protected override void OnRecycle()
{
_partition = null;
_otherPartition = null;
_leafNodes.Dispose();
_leafNodes = null;
if (_otherCandidates != null)
{
_otherCandidates.Dispose();
_otherCandidates = null;
}
Pool.Recycle(this);
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (CompositeShape)sourceShape;
foreach (var geometry in source.Children)
{
Children.Add((IGeometricObject)geometry.Clone());
}
if (source.Partition != null)
{
Partition = source.Partition.Clone();
}
}
public static IEnumerable <Pair <T> > Create(DynamicAabbTree <T> partition,
ISpatialPartition <T> otherPartition, IEnumerable <Node> leafNodes,
ref Vector3F scale, ref Vector3F otherScaleInverse, ref Pose toOther)
{
var enumerable = Pool.Obtain();
enumerable._partition = partition;
enumerable._otherPartition = otherPartition;
enumerable._leafNodes = leafNodes.GetEnumerator();
enumerable._scale = scale;
enumerable._otherScaleInverse = otherScaleInverse;
enumerable._toOther = toOther;
return(enumerable);
}
public static IEnumerable <Pair <T> > Create(BasePartition <T> partition,
ISpatialPartition <T> otherPartition, IEnumerable <T> candidates,
ref Vector3F scale, ref Vector3F otherScaleInverse, ref Pose toOther)
{
var enumerable = Pool.Obtain();
enumerable._partition = partition;
enumerable._otherPartition = otherPartition;
enumerable._candidates = candidates.GetEnumerator();
enumerable._scale = scale;
enumerable._otherScaleInverse = otherScaleInverse;
enumerable._toOther = toOther;
return(enumerable);
}
public void GetClosestPointCandidates(Vector3F scale, Pose pose, ISpatialPartition <T> otherPartition, Vector3F otherScale, Pose otherPose, Func <T, T, float> callback)
{
if (otherPartition == null)
{
throw new ArgumentNullException("otherPartition");
}
if (callback == null)
{
throw new ArgumentNullException("callback");
}
// Make sure we are up-to-date.
var otherBasePartition = otherPartition as BasePartition <T>;
if (otherBasePartition != null)
{
otherBasePartition.UpdateInternal();
}
else
{
otherPartition.Update(false);
}
UpdateInternal();
if (_root == null)
{
return;
}
if (otherPartition is AdaptiveAabbTree <T> )
{
// ----- AdaptiveAabbTree<T> vs. AdaptiveAabbTree<T>
// (Transform second partition into local space.)
var otherTree = (AdaptiveAabbTree <T>)otherPartition;
float closestPointDistanceSquared = float.PositiveInfinity;
GetClosestPointCandidatesImpl(_root, scale, otherTree._root, otherScale, pose.Inverse * otherPose, callback, ref closestPointDistanceSquared);
}
else if (otherPartition is ISupportClosestPointQueries <T> )
{
// ----- AdaptiveAabbTree<T> vs. ISupportClosestPointQueries<T>
GetClosestPointCandidatesImpl(scale, pose, (ISupportClosestPointQueries <T>)otherPartition, otherScale, otherPose, callback);
}
else
{
// ----- AdaptiveAabbTree<T> vs. *
GetClosestPointCandidatesImpl(otherPartition, callback);
}
}
private void GetClosestPointCandidatesImpl(ISpatialPartition <T> otherPartition, Func <T, T, float> callback)
{
// Return all possible pairs.
foreach (var itemA in Items)
{
foreach (var itemB in otherPartition)
{
float closestPointDistanceSquared = callback(itemA, itemB);
if (closestPointDistanceSquared < 0)
{
// closestPointDistanceSquared == -1 indicates early exit.
return;
}
}
}
}
/// <inheritdoc/>
public virtual IEnumerable<Pair<T>> GetOverlaps(Vector3 scale, Pose pose, ISpatialPartition<T> otherPartition, Vector3 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.
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 candidates = GetOverlaps(otherAabb);
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);
}
/// <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
}
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.");
}
/// <inheritdoc/>
public override 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 (_root == null)
{
return(LinqHelper.Empty <Pair <T> >());
}
var otherTree = otherPartition as AdaptiveAabbTree <T>;
if (otherTree == null)
{
// AdaptiveAabbTree<T> vs. ISpatialPartition<T>.
return(GetOverlapsImpl(otherPartition));
}
else
{
// AdaptiveAabbTree<T> vs. AdaptiveAabbTree<T>
if (otherTree._root == null)
{
return(LinqHelper.Empty <Pair <T> >());
}
return(GetOverlapsImpl(otherTree));
}
}
/// <summary>
/// Initializes a new instance of the <see cref="DualPartition{T}"/> class using the given
/// pair of spatial partitions.
/// </summary>
/// <param name="staticPartition">
/// The spatial partition used for static/sleeping objects.
/// </param>
/// <param name="dynamicPartition">
/// The spatial partition used for dynamic partition.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="staticPartition"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="dynamicPartition"/> is <see langword="null"/>.
/// </exception>
public DualPartition(ISpatialPartition <T> staticPartition, ISpatialPartition <T> dynamicPartition)
{
if (staticPartition == null)
{
throw new ArgumentNullException("staticPartition");
}
if (dynamicPartition == null)
{
throw new ArgumentNullException("dynamicPartition");
}
StaticPartition = staticPartition;
DynamicPartition = dynamicPartition;
_stage0 = new HashSet <T>();
_stage1 = new HashSet <T>();
_currentStage = _stage0;
_previousStage = _stage1;
}
public void SetUp()
{
RandomHelper.Random = new Random(1234567);
_testObjects.Clear();
_testObjectsOfPartition2.Clear();
TestObject.NextId = 0;
_testObjectsOfPartition2.Add(new TestObject(GetRandomAabb()));
_testObjectsOfPartition2.Add(new TestObject(GetRandomAabb()));
_testObjectsOfPartition2.Add(new TestObject(GetRandomAabb()));
_testObjectsOfPartition2.Add(new TestObject(GetRandomAabb()));
_partition2 = new AabbTree<int>();
_partition2.GetAabbForItem = GetAabbOfTestObjectOfPartition2;
_partition2.Add(0);
_partition2.Add(1);
_partition2.Add(2);
_partition2.Add(3);
_conservativeAabb = false;
_conservativeOverlaps = false;
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
#if XNA || MONOGAME
/// <summary>
/// Sets the spatial partition. (For use by the content pipeline only.)
/// </summary>
/// <param name="partition">The spatial partition.</param>
/// <remarks>
/// This method is used internally to directly set the spatial partition. The spatial partition
/// might already be initialized and should not be invalidated.
/// </remarks>
internal void SetPartition(ISpatialPartition<int> partition)
{
if (partition != null)
{
_partition = partition;
_partition.GetAabbForItem = i => Children[i].Aabb;
// ----- Validate spatial partition.
// Some spatial partitions, such as the CompressedAabbTree, are pre-initialized when
// loaded via content pipeline. Other spatial partitions need to be initialized manually.
int numberOfChildren = Children.Count;
if (_partition.Count != numberOfChildren)
{
// The partition is not initialized.
_partition.Clear();
for (int i = 0; i < numberOfChildren; i++)
_partition.Add(i);
_partition.Update(false);
}
else
{
// The partition is already initialized.
Debug.Assert(Enumerable.Range(0, numberOfChildren).All(_partition.Contains), "Invalid partition. The pre-initialized partition does not contain the same children as the CompositeShape.");
}
}
}
/// <summary>
/// Initializes a new instance of the <see cref="TriangleMeshShape"/> class from the given
/// triangle mesh.
/// </summary>
/// <param name="mesh">The mesh.</param>
/// <param name="enableContactWelding">
/// If set to <see langword="true"/> contact welding is enabled; otherwise, the shape will not
/// use contact welding. See <see cref="EnableContactWelding"/> for more information.
/// </param>
/// <param name="partition">
/// The spatial partition (see <see cref="Partition"/>). Can be <see langword="null"/> if no
/// partition should be used.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="mesh"/> is <see langword="null"/>.
/// </exception>
public TriangleMeshShape(ITriangleMesh mesh, bool enableContactWelding, ISpatialPartition<int> partition)
{
if (mesh == null)
throw new ArgumentNullException("mesh");
_mesh = mesh;
Partition = partition;
EnableContactWelding = enableContactWelding;
}
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 CloneWithPartition(CompositeShape compositeShape, ISpatialPartition<int> partition)
{
compositeShape.Partition = partition;
CompositeShape clone = compositeShape.Clone() as CompositeShape;
Assert.IsNotNull(clone);
Assert.AreEqual(10, clone.Children.Count);
for (int i = 0; i < 10; i++)
{
Assert.IsNotNull(clone.Children[i]);
Assert.AreNotSame(compositeShape.Children[i], clone.Children[i]);
Assert.IsTrue(clone.Children[i] is GeometricObject);
Assert.AreEqual(compositeShape.Children[i].Pose, clone.Children[i].Pose);
Assert.IsNotNull(clone.Children[i].Shape);
Assert.AreNotSame(compositeShape.Children[i].Shape, clone.Children[i].Shape);
Assert.IsTrue(clone.Children[i].Shape is PointShape);
Assert.AreEqual(((PointShape)compositeShape.Children[i].Shape).Position, ((PointShape)clone.Children[i].Shape).Position);
}
Assert.IsNotNull(clone.Partition);
Assert.IsInstanceOf(partition.GetType(), clone.Partition);
Assert.AreEqual(compositeShape.Children.Count, clone.Partition.Count);
Assert.AreNotSame(partition, clone.Partition);
}
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 TestAabb(ISpatialPartition<int> partition)
{
if (_testObjects.Count == 0)
return;
// Compute desired result.
var desiredAabb = _testObjects[0].Aabb;
_testObjects.ForEach(obj => desiredAabb.Grow(obj.Aabb));
// The AABB of the spatial partition can be slightly bigger.
// E.g. the CompressedAabbTree adds a margin to avoid divisions by zero.
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Minimum.X));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Minimum.Y));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Minimum.Z));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Maximum.X));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Maximum.Y));
Assert.IsTrue(Numeric.IsFinite(partition.Aabb.Maximum.Z));
if (_conservativeAabb)
{
// AABB can be bigger than actual objects.
Assert.IsTrue(partition.Aabb.Contains(desiredAabb), "Wrong AABB: AABB is too small.");
}
else
{
// The AABB should be identical.
Assert.IsTrue(Vector3F.AreNumericallyEqual(desiredAabb.Minimum, partition.Aabb.Minimum));
Assert.IsTrue(Vector3F.AreNumericallyEqual(desiredAabb.Maximum, partition.Aabb.Maximum));
}
}
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.");
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (CompositeShape)sourceShape;
foreach (var geometry in source.Children)
Children.Add((IGeometricObject)geometry.Clone());
if (source.Partition != null)
Partition = source.Partition.Clone();
}
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 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 CloneMeshWithPartition(ISpatialPartition<int> partition)
{
TriangleMeshShape meshShape = (TriangleMeshShape)_meshShape.Clone();
meshShape.Partition = partition;
TriangleMeshShape clone = meshShape.Clone() as TriangleMeshShape;
Assert.IsNotNull(clone);
Assert.IsNotNull(clone.Mesh);
Assert.IsTrue(clone.Mesh is TriangleMesh);
Assert.AreSame(_mesh, clone.Mesh);
Assert.AreEqual(_mesh.NumberOfTriangles, clone.Mesh.NumberOfTriangles);
for (int i = 0; i < _mesh.NumberOfTriangles; i++)
{
Triangle t = _mesh.GetTriangle(i);
Triangle tCloned = clone.Mesh.GetTriangle(i);
Assert.AreEqual(t.Vertex0, tCloned.Vertex0);
Assert.AreEqual(t.Vertex1, tCloned.Vertex1);
Assert.AreEqual(t.Vertex2, tCloned.Vertex2);
}
Assert.IsNotNull(clone.Partition);
Assert.IsInstanceOf(partition.GetType(), clone.Partition);
Assert.AreEqual(_mesh.NumberOfTriangles, clone.Partition.Count);
Assert.AreNotSame(partition, clone.Partition);
}
private void GetClosestPointCandidatesImpl(ISpatialPartition<int> otherPartition, Func<int, int, float> callback)
{
// Return all possible pairs.
foreach (Node node in _nodes)
{
if (node.IsLeaf)
{
foreach (var otherItem in otherPartition)
{
// TODO: We could compute the AABBs, the minDistance of the AABBs and ignore
// this pair if the minDistance of the AABBs is greater than the current
// closestPointDistance.
float closestPointDistanceSquared = callback(node.Item, otherItem);
if (closestPointDistanceSquared < 0)
{
// closestPointDistanceSquared == -1 indicates early exit.
return;
}
}
}
}
}
