private void GetClosestPointCandidatesImpl(Vector3 scale, Pose pose, ISupportClosestPointQueries<T> otherPartition, Vector3 otherScale, Pose otherPose, Func<T, T, float> callback) { // Test leaf nodes against other partition. // Use a wrapper for the callback to reduce the parameters from Func<T, T, float> to // Func<T, float>. ClosestPointCallbackWrapper<T> callbackWrapper = ClosestPointCallbackWrapper<T>.Create(); callbackWrapper.OriginalCallback = callback; // Prepare transformation to transform leaf AABBs into local space of other partition. Pose toOther = otherPose.Inverse * pose; Vector3 otherScaleInverse = Vector3.One / otherScale; float closestPointDistanceSquared = float.PositiveInfinity; foreach (var leaf in _leaves.Values) { callbackWrapper.Item = leaf.Item; // Transform AABB into local space of other partition. Aabb aabb = leaf.Aabb.GetAabb(scale, toOther); aabb.Scale(otherScaleInverse); closestPointDistanceSquared = otherPartition.GetClosestPointCandidates(aabb, closestPointDistanceSquared, callbackWrapper.Callback); if (closestPointDistanceSquared < 0) { // closestPointDistanceSquared == -1 indicates early exit. break; } } callbackWrapper.Recycle(); }
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); }
public void Scale() { Aabb aabb = new Aabb(new Vector3F(1, 2, 3), new Vector3F(4, 5, 6)); aabb.Scale(new Vector3F(-2, 3, 4)); Assert.AreEqual(new Aabb(new Vector3F(-8, 6, 12), new Vector3F(-2, 15, 24)), aabb); aabb = new Aabb(new Vector3F(1, 2, 3), new Vector3F(4, 5, 6)); aabb.Scale(new Vector3F(2, -3, 4)); Assert.AreEqual(new Aabb(new Vector3F(2, -15, 12), new Vector3F(8, -6, 24)), aabb); aabb = new Aabb(new Vector3F(1, 2, 3), new Vector3F(4, 5, 6)); aabb.Scale(new Vector3F(2, 3, -4)); Assert.AreEqual(new Aabb(new Vector3F(2, 6, -24), new Vector3F(8, 15, -12)), aabb); }
/// <summary> /// Makes an AABB check for the two node AABBs where the second has a pose and scale. /// </summary> /// <param name="nodeA">The first AABB node.</param> /// <param name="scaleA">The scale of the first AABB node.</param> /// <param name="nodeB">The second AABB node.</param> /// <param name="scaleB">The scale of the second AABB node.</param> /// <param name="poseB">The pose of the second AABB node relative to the first.</param> /// <returns> /// <see langword="true"/> if the AABBs have contact; otherwise, <see langword="false"/>. /// </returns> private static bool HaveAabbContact(Node nodeA, Vector3 scaleA, Node nodeB, Vector3 scaleB, Pose poseB) { // Scale AABB of A. Aabb aabbA = nodeA.Aabb; aabbA.Scale(scaleA); // Scale AABB of B. Aabb aabbB = nodeB.Aabb; aabbB.Scale(scaleB); // Convert AABB of B to OBB in local space of A. Vector3 boxExtentB = aabbB.Extent; Pose poseBoxB = poseB * new Pose(aabbB.Center); // Test AABB of A against OBB. return GeometryHelper.HaveContact(aabbA, boxExtentB, poseBoxB, false); // We do not make edge tests. }
protected override bool OnNext(out Pair <int> current) { while (true) { if (_otherCandidates == null) { if (_leafNodes.MoveNext()) { var leaf = _leafNodes.Current; _leafAabb = _partition.GetAabb(leaf); _leafAabb = _leafAabb.GetAabb(_scale, _toOther); _leafAabb.Scale(_otherScaleInverse); _otherCandidates = _otherPartition.GetOverlaps(_leafAabb).GetEnumerator(); } else { current = default(Pair <int>); return(false); } } while (_otherCandidates.MoveNext()) { var leaf = _leafNodes.Current; var otherCandidate = _otherCandidates.Current; var overlap = new Pair <int>(leaf.Item, otherCandidate); if (_partition.Filter == null || _partition.Filter.Filter(overlap)) { current = overlap; return(true); } } _otherCandidates.Dispose(); _otherCandidates = null; } }
/// <inheritdoc/> public override Aabb GetAabb(Vector3F scale, Pose pose) { // Recompute local cached AABB if it is invalid. if (Numeric.IsNaN(_minHeight)) { // Find min and max height. // TODO: We could cache that beforehand. _minHeight = float.PositiveInfinity; _maxHeight = float.NegativeInfinity; foreach (float height in _samples) { if (height < _minHeight) _minHeight = height; if (height > _maxHeight) _maxHeight = height; } } Vector3F minimum = new Vector3F(_originX, _minHeight, _originZ); Vector3F maximum = new Vector3F(_originX + _widthX, _maxHeight, _originZ + _widthZ); // Apply scale. var scaledLocalAabb = new Aabb(minimum, maximum); scaledLocalAabb.Scale(scale); // Add depth after scaling because scaleY = 0 makes sense to flatten the height field // but the bounding box should have a height > 0 to avoid tunneling. scaledLocalAabb.Minimum.Y = Math.Min(scaledLocalAabb.Minimum.Y - _depth, -_depth); // Apply pose. return scaledLocalAabb.GetAabb(pose); }
/// <summary> /// Gets all items that are candidates for the smallest closest-point distance to items in a /// given partition. (Internal, recursive.) /// </summary> /// <param name="nodeA">The first AABB node.</param> /// <param name="scaleA">The scale of the first AABB node.</param> /// <param name="nodeB">The second AABB node.</param> /// <param name="scaleB">The scale of the second AABB node.</param> /// <param name="poseB">The pose of the second AABB node relative to the first.</param> /// <param name="callback"> /// The callback that is called with each found candidate item. The method must compute the /// closest-point on the candidate item and return the squared closest-point distance. /// </param> /// <param name="closestPointDistanceSquared"> /// The squared of the current closest-point distance. /// </param> private void GetClosestPointCandidatesImpl(Node nodeA, Vector3F scaleA, Node nodeB, Vector3F scaleB, Pose poseB, Func <T, T, float> callback, ref float closestPointDistanceSquared) { // closestPointDistanceSquared == -1 indicates early exit. if (nodeA == null || nodeB == null || closestPointDistanceSquared < 0) { // Abort. return; } nodeA.IsActive = true; nodeB.IsActive = true; // If we have a contact, it is not necessary to examine nodes with no AABB contact // because they cannot give a closer point pair. if (closestPointDistanceSquared == 0 && !HaveAabbContact(nodeA, scaleA, nodeB, scaleB, poseB)) { return; } if (nodeA.IsLeaf && nodeB.IsLeaf) { // Leaf vs leaf. if (Filter == null || Filter.Filter(new Pair <T>(nodeA.Item, nodeB.Item))) { var leafDistanceSquared = callback(nodeA.Item, nodeB.Item); closestPointDistanceSquared = Math.Min(leafDistanceSquared, closestPointDistanceSquared); } return; } // Determine which one to split: // If B is a leaf, we have to split A. OR // If A can be split and is bigger than B, we split A. if (nodeB.IsLeaf || (!nodeA.IsLeaf && IsABiggerThanB(nodeA, scaleA, nodeB, scaleB))) { #region ----- Split A ----- SplitIfNecessary(nodeA); Node leftChild = nodeA.LeftChild; Node rightChild = nodeA.RightChild; if (closestPointDistanceSquared == 0) { // We have contact, so we must examine all children. GetClosestPointCandidatesImpl(leftChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared); GetClosestPointCandidatesImpl(rightChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared); return; } // No contact. Use lower bound estimates to search the best nodes first. // TODO: Optimize: We do not need to call GeometryHelper.GetDistanceLowerBoundSquared for OBBs. We have AABB + OBB. // Scale AABB of B. Aabb aabbB = nodeB.Aabb; aabbB.Scale(scaleB); // Convert AABB of B to OBB in local space of A. Vector3F boxExtentB = aabbB.Extent; Pose poseBoxB = poseB * new Pose(aabbB.Center); // Scale left child AABB of A. Aabb leftChildAabb = leftChild.Aabb; leftChildAabb.Scale(scaleA); // Convert left child AABB of A to OBB in local space of A. Vector3F leftChildBoxExtent = leftChildAabb.Extent; Pose leftChildBoxPose = new Pose(leftChildAabb.Center); // Compute lower bound for distance to left child. float minDistanceLeft = GeometryHelper.GetDistanceLowerBoundSquared(leftChildBoxExtent, leftChildBoxPose, boxExtentB, poseBoxB); // Scale right child AABB of A. Aabb rightChildAabb = rightChild.Aabb; rightChildAabb.Scale(scaleA); // Convert right child AABB of A to OBB in local space of A. Vector3F rightChildBoxExtent = rightChildAabb.Extent; Pose rightChildBoxPose = new Pose(rightChildAabb.Center); // Compute lower bound for distance to right child. float minDistanceRight = GeometryHelper.GetDistanceLowerBoundSquared(rightChildBoxExtent, rightChildBoxPose, boxExtentB, poseBoxB); if (minDistanceLeft < minDistanceRight) { // Stop if other child cannot improve result. // Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN. if (minDistanceLeft > closestPointDistanceSquared) { return; } // Handle left first. GetClosestPointCandidatesImpl(leftChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared); // Stop if other child cannot improve result. // Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN. if (minDistanceRight > closestPointDistanceSquared) { return; } GetClosestPointCandidatesImpl(rightChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared); } else { // Stop if other child cannot improve result. // Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN. if (minDistanceRight > closestPointDistanceSquared) { return; } // Handle right first. GetClosestPointCandidatesImpl(rightChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared); // Stop if other child cannot improve result. // Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN. if (minDistanceLeft > closestPointDistanceSquared) { return; } GetClosestPointCandidatesImpl(leftChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared); } #endregion } else { #region ----- Split B ----- SplitIfNecessary(nodeB); Node leftChildB = nodeB.LeftChild; Node rightChildB = nodeB.RightChild; if (closestPointDistanceSquared == 0) { // We have contact, so we must examine all children. GetClosestPointCandidatesImpl(nodeA, scaleA, leftChildB, scaleB, poseB, callback, ref closestPointDistanceSquared); GetClosestPointCandidatesImpl(nodeA, scaleA, rightChildB, scaleB, poseB, callback, ref closestPointDistanceSquared); } else { // No contact. Use lower bound estimates to search the best nodes first. // TODO: Optimize: We do not need to call GeometryHelper.GetDistanceLowerBoundSquared for OBBs. We have AABB + OBB. // Scale AABB of A. Aabb aabbA = nodeA.Aabb; aabbA.Scale(scaleA); // Convert AABB of A to OBB in local space of A. Vector3F boxExtentA = aabbA.Extent; Pose poseBoxA = new Pose(aabbA.Center); // Scale left child AABB of B. Aabb leftChildAabb = leftChildB.Aabb; leftChildAabb.Scale(scaleB); // Convert left child AABB of B to OBB in local space of A. Vector3F childBoxExtent = leftChildAabb.Extent; Pose poseLeft = poseB * new Pose(leftChildAabb.Center); // Compute lower bound for distance to left child. float minDistanceLeft = GeometryHelper.GetDistanceLowerBoundSquared(childBoxExtent, poseLeft, boxExtentA, poseBoxA); // Scale right child AABB of B. Aabb rightChildAabb = rightChildB.Aabb; rightChildAabb.Scale(scaleB); // Convert right child AABB of B to OBB in local space of A. childBoxExtent = rightChildAabb.Extent; Pose poseRight = poseB * new Pose(rightChildAabb.Center); // Compute lower bound for distance to right child. float minDistanceRight = GeometryHelper.GetDistanceLowerBoundSquared(childBoxExtent, poseRight, boxExtentA, poseBoxA); if (minDistanceLeft < minDistanceRight) { // Stop if other child cannot improve result. // Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN. if (minDistanceLeft > closestPointDistanceSquared) { return; } // Handle left first. GetClosestPointCandidatesImpl(nodeA, scaleA, leftChildB, scaleB, poseB, callback, ref closestPointDistanceSquared); // Stop if other child cannot improve result. // Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN. if (minDistanceRight > closestPointDistanceSquared) { return; } GetClosestPointCandidatesImpl(nodeA, scaleA, rightChildB, scaleB, poseB, callback, ref closestPointDistanceSquared); } else { // Stop if other child cannot improve result. // Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN. if (minDistanceRight > closestPointDistanceSquared) { return; } // Handle right first. GetClosestPointCandidatesImpl(nodeA, scaleA, rightChildB, scaleB, poseB, callback, ref closestPointDistanceSquared); // Stop if other child cannot improve result. // Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN. if (minDistanceLeft > closestPointDistanceSquared) { return; } GetClosestPointCandidatesImpl(nodeA, scaleA, leftChildB, scaleB, poseB, callback, ref closestPointDistanceSquared); } } #endregion } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { CollisionObject collisionObjectA = contactSet.ObjectA; CollisionObject collisionObjectB = contactSet.ObjectB; IGeometricObject geometricObjectA = collisionObjectA.GeometricObject; IGeometricObject geometricObjectB = collisionObjectB.GeometricObject; // Object A should be the composite, swap objects if necessary. // When testing CompositeShape vs. CompositeShape with BVH, object A should be the // CompositeShape with BVH. CompositeShape compositeShapeA = geometricObjectA.Shape as CompositeShape; CompositeShape compositeShapeB = geometricObjectB.Shape as CompositeShape; bool swapped = false; if (compositeShapeA == null) { // Object A is something else. Object B must be a composite shape. swapped = true; } else if (compositeShapeA.Partition == null && compositeShapeB != null && compositeShapeB.Partition != null) { // Object A has no BVH, object B is CompositeShape with BVH. swapped = true; } if (swapped) { MathHelper.Swap(ref collisionObjectA, ref collisionObjectB); MathHelper.Swap(ref geometricObjectA, ref geometricObjectB); MathHelper.Swap(ref compositeShapeA, ref compositeShapeB); } // Check if collision objects shapes are correct. if (compositeShapeA == null) { throw new ArgumentException("The contact set must contain a composite shape.", "contactSet"); } // Assume no contact. contactSet.HaveContact = false; Vector3 scaleA = geometricObjectA.Scale; Vector3 scaleB = geometricObjectB.Scale; // Check if transforms are supported. if (compositeShapeA != null && // When object A is a CompositeShape (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z) && // non-uniform scaling is not supported compositeShapeA.Children.Any(child => child.Pose.HasRotation)) // when a child has a local rotation. { // Note: Any() creates garbage, but non-uniform scalings should not be used anyway. throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported."); } // Same check for object B. if (compositeShapeB != null && (scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z) && compositeShapeB.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway. { throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported."); } // ----- A few fixed objects which are reused to avoid GC garbage. var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain(); var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain(); // Create a test contact set and initialize with dummy objects. // (The actual collision objects are set below.) var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB); var testGeometricObjectA = TestGeometricObject.Create(); var testGeometricObjectB = TestGeometricObject.Create(); try { if (compositeShapeA.Partition != null && (type != CollisionQueryType.ClosestPoints || compositeShapeA.Partition is ISupportClosestPointQueries <int>)) { if (compositeShapeB != null && compositeShapeB.Partition != null) { Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine."); if (type != CollisionQueryType.ClosestPoints) { // ----- Boolean or Contact Query // Heuristic: Test large BVH vs. small BVH. Aabb aabbOfA = geometricObjectA.Aabb; Aabb aabbOfB = geometricObjectB.Aabb; float largestExtentA = aabbOfA.Extent.LargestComponent; float largestExtentB = aabbOfB.Extent.LargestComponent; IEnumerable <Pair <int> > overlaps; bool overlapsSwapped = largestExtentA < largestExtentB; if (overlapsSwapped) { overlaps = compositeShapeB.Partition.GetOverlaps( scaleB, geometricObjectB.Pose, compositeShapeA.Partition, scaleA, geometricObjectA.Pose); } else { overlaps = compositeShapeA.Partition.GetOverlaps( scaleA, geometricObjectA.Pose, compositeShapeB.Partition, scaleB, geometricObjectB.Pose); } foreach (var overlap in overlaps) { if (type == CollisionQueryType.Boolean && contactSet.HaveContact) { break; // We can abort early. } AddChildChildContacts( contactSet, overlapsSwapped ? overlap.Second : overlap.First, overlapsSwapped ? overlap.First : overlap.Second, type, testContactSet, testCollisionObjectA, testGeometricObjectA, testCollisionObjectB, testGeometricObjectB); } } else { // Closest-Point Query var callback = ClosestPointsCallbacks.Obtain(); callback.CollisionAlgorithm = this; callback.Swapped = false; callback.ContactSet = contactSet; callback.TestCollisionObjectA = testCollisionObjectA; callback.TestCollisionObjectB = testCollisionObjectB; callback.TestGeometricObjectA = testGeometricObjectA; callback.TestGeometricObjectB = testGeometricObjectB; callback.TestContactSet = testContactSet; ((ISupportClosestPointQueries <int>)compositeShapeA.Partition) .GetClosestPointCandidates( scaleA, geometricObjectA.Pose, compositeShapeB.Partition, scaleB, geometricObjectB.Pose, callback.HandlePair); ClosestPointsCallbacks.Recycle(callback); } } else { // Compute AABB of B in local space of the CompositeShape. Aabb aabbBInA = geometricObjectB.Shape.GetAabb( scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose); // Apply inverse scaling to do the AABB checks in the unscaled local space of A. aabbBInA.Scale(Vector3.One / scaleA); if (type != CollisionQueryType.ClosestPoints) { // Boolean or Contact Query foreach (var childIndex in compositeShapeA.Partition.GetOverlaps(aabbBInA)) { if (type == CollisionQueryType.Boolean && contactSet.HaveContact) { break; // We can abort early. } AddChildContacts( contactSet, swapped, childIndex, type, testContactSet, testCollisionObjectA, testGeometricObjectA); } } else if (type == CollisionQueryType.ClosestPoints) { // Closest-Point Query var callback = ClosestPointsCallbacks.Obtain(); callback.CollisionAlgorithm = this; callback.Swapped = swapped; callback.ContactSet = contactSet; callback.TestCollisionObjectA = testCollisionObjectA; callback.TestCollisionObjectB = testCollisionObjectB; callback.TestGeometricObjectA = testGeometricObjectA; callback.TestGeometricObjectB = testGeometricObjectB; callback.TestContactSet = testContactSet; ((ISupportClosestPointQueries <int>)compositeShapeA.Partition) .GetClosestPointCandidates( aabbBInA, float.PositiveInfinity, callback.HandleItem); ClosestPointsCallbacks.Recycle(callback); } } } else { // Compute AABB of B in local space of the composite. Aabb aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose); // Apply inverse scaling to do the AABB checks in the unscaled local space of A. aabbBInA.Scale(Vector3.One / scaleA); // Go through list of children and find contacts. int numberOfChildGeometries = compositeShapeA.Children.Count; for (int i = 0; i < numberOfChildGeometries; i++) { IGeometricObject child = compositeShapeA.Children[i]; // NOTE: For closest-point queries we could be faster estimating a search space. // See TriangleMeshAlgorithm or BVH queries. // But the current implementation is sufficient. If the CompositeShape is more complex // the user should be using spatial partitions anyway. // For boolean or contact queries, we make an AABB test first. // For closest points where we have not found a contact yet, we have to search // all children. if ((type == CollisionQueryType.ClosestPoints && !contactSet.HaveContact) || GeometryHelper.HaveContact(aabbBInA, child.Shape.GetAabb(child.Scale, child.Pose))) { // TODO: We could compute the minDistance of the child AABB and the AABB of the // other shape. If the minDistance is greater than the current closestPairDistance // we can ignore this pair. - This could be a performance boost. // Get contacts/closest pairs of this child. AddChildContacts( contactSet, swapped, i, type, testContactSet, testCollisionObjectA, testGeometricObjectA); // We have contact and stop for boolean queries. if (contactSet.HaveContact && type == CollisionQueryType.Boolean) { break; } } } } } finally { Debug.Assert(collisionObjectA.GeometricObject.Shape == compositeShapeA, "Shape was altered and not restored."); testContactSet.Recycle(); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB); testGeometricObjectB.Recycle(); testGeometricObjectA.Recycle(); } }