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