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; } } } } }