/// <summary>
/// Gets the bounding sphere of a geometric object.
/// </summary>
/// <param name="geometricObject">The geometric object.</param>
/// <param name="radius">The radius of the bounding sphere.</param>
/// <param name="center">The center of the bounding sphere in world space.</param>
internal static void GetBoundingSphere(IGeometricObject geometricObject, out float radius, out Vector3F center)
{
// Get sphere from AABB.
Aabb aabb = geometricObject.Aabb;
center = aabb.Center;
radius = aabb.Extent.Length * 0.5f;
}
/// <summary>
/// Initializes a new instance of the <see cref="CollisionObject"/> class with the given
/// geometric object.
/// </summary>
/// <param name="geometricObject">
/// The geometric object (see property <see cref="GeometricObject"/>).
/// </param>
public CollisionObject(IGeometricObject geometricObject)
{
GeometricObject = geometricObject;
Enabled = true;
Changed = true;
Type = CollisionObjectType.Default;
}
/// <summary>
/// Gets the radius for a bounding sphere centered at the position of the geometric object.
/// </summary>
/// <param name="geometricObject">The geometric object.</param>
/// <returns>
/// The radius of the bounding sphere if the sphere's center is identical to the origin of the
/// geometric object.
/// </returns>
internal static float GetBoundingRadius(IGeometricObject geometricObject)
{
// Get offset bounding sphere radius + sphere offset.
Vector3F center;
float radius;
GetBoundingSphere(geometricObject, out radius, out center);
return (center - geometricObject.Pose.Position).Length + radius;
}
/// <summary>
/// Gets a scissor rectangle that encloses the specified geometric object.
/// </summary>
/// <param name="cameraNode">The camera node.</param>
/// <param name="viewport">The viewport.</param>
/// <param name="geometricObject">The geometric object.</param>
/// <returns>The scissor rectangle.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="cameraNode"/> or <paramref name="geometricObject"/> is
/// <see langword="null"/>.
/// </exception>
public static Rectangle GetScissorRectangle(CameraNode cameraNode, Viewport viewport, IGeometricObject geometricObject)
{
var rectangle = GetViewportRectangle(cameraNode, viewport, geometricObject);
rectangle.X += viewport.X;
rectangle.Y += viewport.Y;
return rectangle;
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
public VehicleCameraObject(IGeometricObject vehicle, IServiceLocator services)
{
Name = "VehicleCamera";
_vehicle = vehicle;
_services = services;
_inputService = services.GetInstance<IInputService>();
}
// Creates the graphics resources (e.g. the vertex buffer).
public void LoadContent(GraphicsDevice graphicsDevice, IGeometricObject geometricObject)
{
// Create a mesh for the given shape. (The arguments define the desired resolution if the
// mesh is only approximated - for example for a sphere.)
TriangleMesh mesh = geometricObject.Shape.GetMesh(0.01f, 3);
// Abort if we have nothing to draw. (This happens for "EmptyShapes".)
if (mesh.Vertices.Count == 0)
return;
// Apply the scaling that is defined in the geometric object.
if (geometricObject.Scale != Vector3F.One)
mesh.Transform(Matrix44F.CreateScale(geometricObject.Scale));
_numberOfTriangles = mesh.NumberOfTriangles;
// Create vertex data for a triangle list.
VertexPositionNormalTexture[] vertices = new VertexPositionNormalTexture[mesh.NumberOfTriangles * 3];
// Create vertex normals. Do not merge normals if the angle between the triangle normals
// is > 70°.
Vector3F[] normals = mesh.ComputeNormals(false, MathHelper.ToRadians(70));
// Loop over all triangles and copy vertices to the vertices array.
for (int i = 0; i < _numberOfTriangles; i++)
{
// Get next triangle of the mesh.
Triangle triangle = mesh.GetTriangle(i);
// Add new vertex data.
// DigitalRune.Geometry uses counter-clockwise front faces. XNA uses
// clockwise front faces (CullMode.CullCounterClockwiseFace) per default.
// Therefore we change the vertex orientation of the triangles.
// We could also keep the vertex order and change the CullMode to CullClockwiseFace.
vertices[i * 3 + 0] = new VertexPositionNormalTexture(
(Vector3)triangle.Vertex0,
(Vector3)normals[i * 3 + 0],
Vector2.Zero);
vertices[i * 3 + 1] = new VertexPositionNormalTexture(
(Vector3)triangle.Vertex2, // triangle.Vertex2 instead of triangle.Vertex1 to change vertex order!
(Vector3)normals[i * 3 + 2],
Vector2.Zero);
vertices[i * 3 + 2] = new VertexPositionNormalTexture(
(Vector3)triangle.Vertex1, // triangle.Vertex1 instead of triangle.Vertex2 to change vertex order!
(Vector3)normals[i * 3 + 1],
Vector2.Zero);
}
// Create a vertex buffer.
_vertexBuffer = new VertexBuffer(
graphicsDevice,
vertices[0].GetType(),
vertices.Length,
BufferUsage.WriteOnly);
// Fill the vertex buffer.
_vertexBuffer.SetData(vertices);
}
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;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F 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)
{
#region ----- Composite with BVH vs. Composite with BVH -----
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);
}
#endregion
}
else
{
#region ----- Composite with BVH vs. * -----
// 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(Vector3F.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);
}
#endregion
}
}
else
{
#region ----- Composite vs. *-----
// 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(Vector3F.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;
}
}
}
#endregion
}
}
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();
}
}
private void AddTriangleContacts(ContactSet contactSet,
bool swapped,
int triangleIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject,
TriangleShape testTriangle)
{
// Object A should be the triangle mesh.
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
var triangleMeshShape = ((TriangleMeshShape)geometricObjectA.Shape);
Triangle triangle = triangleMeshShape.Mesh.GetTriangle(triangleIndex);
Pose poseA = geometricObjectA.Pose;
Vector3 scaleA = geometricObjectA.Scale;
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), collisionObjectB.GeometricObject.Shape.GetType()];
// Apply scaling.
testTriangle.Vertex0 = triangle.Vertex0 * scaleA;
testTriangle.Vertex1 = triangle.Vertex1 * scaleA;
testTriangle.Vertex2 = triangle.Vertex2 * scaleA;
// Set the shape temporarily to the current triangles.
testGeometricObject.Shape = testTriangle;
testGeometricObject.Scale = Vector3.One;
testGeometricObject.Pose = poseA;
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Make a temporary contact set.
// (Object A and object B should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
{
testContactSet.Reset(collisionObjectB, testCollisionObject);
}
else
{
testContactSet.Reset(testCollisionObject, collisionObjectB);
}
if (type == CollisionQueryType.Boolean)
{
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
}
else
{
// No perturbation test. Most triangle mesh shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// TODO: Copy separating axis info and similar things into triangleContactSet.
// But currently this info is not used in the queries.
// For closest points: If we know that we have a contact, then we can make a
// faster contact query instead of a closest-point query.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
if (testContactSet.HaveContact && testContactSet.Count > 0 && !triangleMeshShape.IsTwoSided)
{
// To compute the triangle normal in world space we take the normal of the unscaled
// triangle and transform the normal with: (M^-1)^T = 1 / scale
Vector3 triangleNormalLocal = Vector3.Cross(triangle.Vertex1 - triangle.Vertex0, triangle.Vertex2 - triangle.Vertex0) / scaleA;
Vector3 triangleNormal = poseA.ToWorldDirection(triangleNormalLocal);
if (triangleNormal.TryNormalize())
{
var preferredNormal = swapped ? -triangleNormal : triangleNormal;
// ----- Remove bad normal.
// Triangles are double sided, but meshes are single sided.
// --> Remove contacts where the contact normal points into the wrong direction.
ContactHelper.RemoveBadContacts(testContactSet, preferredNormal, -Numeric.EpsilonF);
if (testContactSet.Count > 0 && triangleMeshShape.EnableContactWelding)
{
var contactDotTriangle = Vector3.Dot(testContactSet[0].Normal, preferredNormal);
if (contactDotTriangle < WeldingLimit)
{
// Bad normal. Perform welding.
Vector3 contactPositionOnTriangle = swapped
? testContactSet[0].PositionBLocal / scaleA
: testContactSet[0].PositionALocal / scaleA;
Vector3 neighborNormal;
float triangleDotNeighbor;
GetNeighborNormal(triangleIndex, triangle, contactPositionOnTriangle, triangleNormal, triangleMeshShape, poseA, scaleA, out neighborNormal, out triangleDotNeighbor);
if (triangleDotNeighbor < float.MaxValue && Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Normal is not in allowed range.
// Test again in triangle normal direction.
Contact c0 = testContactSet[0];
testContactSet.RemoveAt(0);
testContactSet.Clear();
testContactSet.PreferredNormal = preferredNormal;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
testContactSet.PreferredNormal = Vector3.Zero;
if (testContactSet.Count > 0)
{
Contact c1 = testContactSet[0];
float contact1DotTriangle = Vector3.Dot(c1.Normal, preferredNormal);
// We use c1 instead of c0 if it has lower penetration depth (then it is simply
// better). Or we use c1 if the penetration depth increase is in an allowed range
// and c1 has a normal in the allowed range.
if (c1.PenetrationDepth < c0.PenetrationDepth ||
Numeric.IsGreaterOrEqual(contact1DotTriangle, triangleDotNeighbor) &&
c1.PenetrationDepth < c0.PenetrationDepth + CollisionDetection.ContactPositionTolerance)
{
c0.Recycle();
c0 = c1;
testContactSet.RemoveAt(0);
contactDotTriangle = contact1DotTriangle;
}
}
if (Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Clamp contact to allowed normal:
// We keep the contact position on the mesh and the penetration depth. We set
// a new normal and compute the other related values for this normal.
if (!swapped)
{
var positionAWorld = c0.PositionAWorld;
c0.Normal = neighborNormal;
var positionBWorld = positionAWorld - c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionBLocal = testContactSet.ObjectB.GeometricObject.Pose.ToLocalPosition(positionBWorld);
}
else
{
var positionBWorld = c0.PositionBWorld;
c0.Normal = -neighborNormal;
var positionAWorld = positionBWorld + c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionALocal = testContactSet.ObjectA.GeometricObject.Pose.ToLocalPosition(positionAWorld);
}
}
c0.Recycle();
}
}
}
}
}
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
if (swapped)
{
contact.FeatureB = triangleIndex;
}
else
{
contact.FeatureA = triangleIndex;
}
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
}
private void ComputeCollisionFast(ContactSet contactSet, CollisionQueryType type,
IGeometricObject heightFieldGeometricObject, IGeometricObject convexGeometricObject,
HeightField heightField, ConvexShape convex, bool swapped)
{
Debug.Assert(type != CollisionQueryType.ClosestPoints);
// Assume no contact.
contactSet.HaveContact = false;
// Get scales and poses
Pose heightFieldPose = heightFieldGeometricObject.Pose;
Vector3F heightFieldScale = heightFieldGeometricObject.Scale;
if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0)
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
Pose convexPose = convexGeometricObject.Pose;
Vector3F convexScale = convexGeometricObject.Scale;
// Get a point in the convex. (Could also use center of AABB.)
var convexPoint = convexPose.ToWorldPosition(convex.InnerPoint * convexScale);
// Get height field coordinates.
convexPoint = heightFieldPose.ToLocalPosition(convexPoint);
float xUnscaled = convexPoint.X / heightFieldScale.X;
float zUnscaled = convexPoint.Z / heightFieldScale.Z;
// If convex point is outside height field, abort.
var originX = heightField.OriginX;
var originZ = heightField.OriginZ;
if (xUnscaled < originX || xUnscaled > originX + heightField.WidthX
|| zUnscaled < originZ || zUnscaled > originZ + heightField.WidthZ)
{
return;
}
// Get height and normal.
float height;
Vector3F normal;
int featureIndex;
GetHeight(heightField, xUnscaled, zUnscaled, out height, out normal, out featureIndex);
// Check for holes.
if (Numeric.IsNaN(height))
return;
// Apply scaling.
height *= heightFieldScale.Y;
// Normals are transformed with the inverse transposed matrix --> 1 / scale.
normal = normal / heightFieldScale;
normal.Normalize();
// ----- Now we test convex vs. plane.
// Convert normal to convex space.
normal = heightFieldPose.ToWorldDirection(normal);
var normalInConvex = convexPose.ToLocalDirection(normal);
// Convert plane point to convex space.
Vector3F planePoint = new Vector3F(convexPoint.X, height, convexPoint.Z);
planePoint = heightFieldPose.ToWorldPosition(planePoint);
planePoint = convexPose.ToLocalPosition(planePoint);
// Get convex support point in plane normal direction.
Vector3F supportPoint = convex.GetSupportPoint(-normalInConvex, convexScale);
// Get penetration depth.
float penetrationDepth = Vector3F.Dot((planePoint - supportPoint), normalInConvex);
// Abort if there is no contact.
if (penetrationDepth < 0)
return;
// Abort if object is too deep under the height field.
// This is important for height fields with holes/caves. Without this check
// no objects could enter the cave.
if (penetrationDepth > heightField.Depth)
return;
// We have contact.
contactSet.HaveContact = true;
// Return for boolean queries.
if (type == CollisionQueryType.Boolean)
return;
// Contact position is in the "middle of the penetration".
Vector3F position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2));
if (swapped)
normal = -normal;
// Add contact
var contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
if (swapped)
contact.FeatureB = featureIndex;
else
contact.FeatureA = featureIndex;
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
if (CollisionDetection.FullContactSetPerFrame
&& contactSet.Count < 3)
{
// Trying to create a full contact set.
// We use arbitrary orthonormal values to perturb the normal direction.
var ortho1 = normalInConvex.Orthonormal1;
var ortho2 = normalInConvex.Orthonormal2;
// Test 4 perturbed support directions.
for (int i = 0; i < 4; i++)
{
Vector3F direction;
switch (i)
{
case 0:
direction = -normalInConvex + ortho1;
break;
case 1:
direction = -normalInConvex - ortho1;
break;
case 2:
direction = -normalInConvex + ortho2;
break;
default:
direction = -normalInConvex - ortho2;
break;
}
// Support point vs. plane test as above:
supportPoint = convex.GetSupportPoint(direction, convexScale);
penetrationDepth = Vector3F.Dot((planePoint - supportPoint), normalInConvex);
if (penetrationDepth >= 0)
{
position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2));
contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
}
}
/// <summary>
/// Gets the rectangle that encloses the specified geometric object in the viewport.
/// </summary>
/// <param name="cameraNode">The camera node.</param>
/// <param name="viewport">The viewport.</param>
/// <param name="geometricObject">The geometric object.</param>
/// <returns>The rectangle that encloses <paramref name="geometricObject"/>.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="cameraNode"/> or <paramref name="geometricObject"/> is
/// <see langword="null"/>.
/// </exception>
internal static Rectangle GetViewportRectangle(CameraNode cameraNode, Viewport viewport, IGeometricObject geometricObject)
{
if (cameraNode == null)
throw new ArgumentNullException("cameraNode");
if (geometricObject == null)
throw new ArgumentNullException("geometricObject");
// For a uniformly scaled sphere we can use the specialized GetScissorRectangle method.
var sphereShape = geometricObject.Shape as SphereShape;
if (sphereShape != null)
{
Vector3F scale = geometricObject.Scale;
if (scale.X == scale.Y && scale.Y == scale.Z)
{
return GetViewportRectangle(cameraNode, viewport, geometricObject.Pose.Position,
scale.X * sphereShape.Radius);
}
}
// Relative bounds: (left, top, right, bottom).
Vector4F bounds = GetBounds(cameraNode, geometricObject);
// Rectangle in viewport.
int left = (int)(bounds.X * viewport.Width); // implicit floor()
int top = (int)(bounds.Y * viewport.Height); // implicit floor()
int right = (int)Math.Ceiling(bounds.Z * viewport.Width);
int bottom = (int)Math.Ceiling(bounds.W * viewport.Height);
return new Rectangle(left, top, right - left, bottom - top);
}
//--------------------------------------------------------------
#region Creation and Cleanup
//--------------------------------------------------------------
/// <overloads>
/// <summary>
/// Initializes a new instance of the <see cref="TransformedShape"/> class.
/// </summary>
/// </overloads>
///
/// <summary>
/// Initializes a new instance of the <see cref="TransformedShape"/> class.
/// </summary>
/// <remarks>
/// <see cref="Child"/> is initialized with a <see cref="Child"/>
/// with an <see cref="EmptyShape"/>.
/// </remarks>
public TransformedShape()
{
_child = new GeometricObject(Empty);
_child.PoseChanged += OnChildPoseChanged;
_child.ShapeChanged += OnChildShapeChanged;
}
private static void GetMass(IGeometricObject geometricObject, Vector3F scale, float densityOrMass, bool isDensity, float relativeDistanceThreshold, int iterationLimit,
out float mass, out Vector3F centerOfMass, out Matrix33F inertia)
{
// Computes mass in parent/world space of the geometric object!
// centerOfMass is in world space and inertia is around the CM in world space!
Pose pose = geometricObject.Pose;
if ((scale.X != scale.Y || scale.Y != scale.Z) && pose.HasRotation)
throw new NotSupportedException("NON-UNIFORM scaling of a TransformedShape or a CompositeShape with ROTATED children is not supported.");
if (scale.X < 0 || scale.Y < 0 || scale.Z < 0)
throw new NotSupportedException("Negative scaling is not supported..");
var shape = geometricObject.Shape;
var totalScale = scale * geometricObject.Scale;
// Inertia around center of mass in local space.
Matrix33F inertiaCMLocal;
Vector3F centerOfMassLocal;
var body = geometricObject as RigidBody;
if (body != null)
{
// The geometric object is a rigid body and we use the properties of this body.
if (!Vector3F.AreNumericallyEqual(scale, Vector3F.One))
throw new NotSupportedException("Scaling is not supported when a child geometric object is a RigidBody.");
var massFrame = body.MassFrame;
mass = massFrame.Mass;
centerOfMassLocal = massFrame.Pose.Position;
inertiaCMLocal = massFrame.Pose.Orientation * Matrix33F.CreateScale(massFrame.Inertia) * massFrame.Pose.Orientation.Transposed;
}
else
{
// Compute new mass properties for the shape.
GetMass(shape, totalScale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit,
out mass, out centerOfMassLocal, out inertiaCMLocal);
}
// Get inertia around center of mass in world space:
// The world inertia is equal to: move to local space using the inverse orientation, then
// apply local inertia then move to world space with the orientation matrix.
inertia = pose.Orientation * inertiaCMLocal * pose.Orientation.Transposed;
// Convert center of mass offset in world space. - Do not forget scale!
centerOfMass = pose.ToWorldPosition(centerOfMassLocal) * scale;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Invoke GJK for closest points.
if (type == CollisionQueryType.ClosestPoints)
{
throw new GeometryException("This collision algorithm cannot handle closest-point queries. Use GJK instead.");
}
//if (UseMpr)
//{
// if (_mpr == null)
// _mpr = new MinkowskiPortalRefinement(CollisionDetection);
// _mpr.ComputeCollision(contactSet, type);
// return;
//}
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleShape triangleShapeA = geometricObjectA.Shape as TriangleShape;
TriangleShape triangleShapeB = geometricObjectB.Shape as TriangleShape;
// Check if collision objects shapes are correct.
if (triangleShapeA == null || triangleShapeB == null)
{
throw new ArgumentException("The contact set must contain triangle shapes.", "contactSet");
}
Vector3 scaleA = Vector3.Absolute(geometricObjectA.Scale);
Vector3 scaleB = Vector3.Absolute(geometricObjectB.Scale);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
// Get triangles in world space.
Triangle triangleA;
triangleA.Vertex0 = poseA.ToWorldPosition(triangleShapeA.Vertex0 * scaleA);
triangleA.Vertex1 = poseA.ToWorldPosition(triangleShapeA.Vertex1 * scaleA);
triangleA.Vertex2 = poseA.ToWorldPosition(triangleShapeA.Vertex2 * scaleA);
Triangle triangleB;
triangleB.Vertex0 = poseB.ToWorldPosition(triangleShapeB.Vertex0 * scaleB);
triangleB.Vertex1 = poseB.ToWorldPosition(triangleShapeB.Vertex1 * scaleB);
triangleB.Vertex2 = poseB.ToWorldPosition(triangleShapeB.Vertex2 * scaleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = GeometryHelper.HaveContact(ref triangleA, ref triangleB);
return;
}
Debug.Assert(type == CollisionQueryType.Contacts, "TriangleTriangleAlgorithm cannot handle closest point queries.");
// Assume no contact.
contactSet.HaveContact = false;
Vector3 position, normal;
float penetrationDepth;
if (!GetContact(ref triangleA, ref triangleB, false, false, out position, out normal, out penetrationDepth))
{
return;
}
contactSet.HaveContact = true;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
/// <summary>
/// Resets the collision object. (For internal use only.)
/// </summary>
internal void ResetInternal()
{
Changed = true;
CollisionGroup = 0;
_domain = null;
Enabled = true;
_geometricObject = null;
_type = CollisionObjectType.Default;
_shapeType = ShapeType.Default;
_shape = null;
ShapeTypeChanged = true;
}
/// <inheritdoc/>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> is a
/// <see cref="TriangleMeshShape"/>.
/// </exception>
public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
// Object A should be the triangle mesh, swap objects if necessary.
if (!(objectA.GeometricObject.Shape is TriangleMeshShape))
{
MathHelper.Swap(ref objectA, ref objectB);
MathHelper.Swap(ref targetPoseA, ref targetPoseB);
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
TriangleMeshShape triangleMeshShapeA = geometricObjectA.Shape as TriangleMeshShape;
// Check if collision objects shapes are correct.
if (triangleMeshShapeA == null)
{
throw new ArgumentException("One object must be a triangle mesh.");
}
// Currently mesh vs. mesh CCD is not supported.
if (objectB.GeometricObject.Shape is TriangleMeshShape)
{
return(1);
}
ITriangleMesh triangleMeshA = triangleMeshShapeA.Mesh;
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
Vector3 scaleA = geometricObjectA.Scale;
Vector3 scaleB = geometricObjectB.Scale;
// Get an AABB of the swept B in the space of A.
// This simplified AABB can miss some rotational movement.
// To simplify, we assume that A is static and B is moving relative to A.
// In general, this is not correct! But for CCD we make this simplification.
// We convert everything to the space of A.
var aabbSweptBInA = geometricObjectB.Shape.GetAabb(scaleB, startPoseA.Inverse * startPoseB);
aabbSweptBInA.Grow(geometricObjectB.Shape.GetAabb(scaleB, targetPoseA.Inverse * targetPoseB));
// Use temporary object.
var triangleShape = ResourcePools.TriangleShapes.Obtain();
// (Vertices will be set in the loop below.)
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = triangleShape;
testGeometricObject.Scale = Vector3.One;
testGeometricObject.Pose = startPoseA;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(objectA, testGeometricObject);
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), geometricObjectB.Shape.GetType()];
float timeOfImpact = 1;
if (triangleMeshShapeA.Partition != null)
{
// Apply inverse scaling to do the AABB-tree checks in the unscaled local space of A.
aabbSweptBInA.Scale(Vector3.One / scaleA);
foreach (var triangleIndex in triangleMeshShapeA.Partition.GetOverlaps(aabbSweptBInA))
{
Triangle triangle = triangleMeshA.GetTriangle(triangleIndex);
// Apply scale.
triangle.Vertex0 = triangle.Vertex0 * scaleA;
triangle.Vertex1 = triangle.Vertex1 * scaleA;
triangle.Vertex2 = triangle.Vertex2 * scaleA;
triangleShape.Vertex0 = triangle.Vertex0;
triangleShape.Vertex1 = triangle.Vertex1;
triangleShape.Vertex2 = triangle.Vertex2;
float triangleTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
testCollisionObject, targetPoseA,
objectB, targetPoseB,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, triangleTimeOfImpact);
}
}
else
{
// Test all triangles.
int numberOfTriangles = triangleMeshA.NumberOfTriangles;
for (int triangleIndex = 0; triangleIndex < numberOfTriangles; triangleIndex++)
{
Triangle triangle = triangleMeshA.GetTriangle(triangleIndex);
// Apply scale.
triangle.Vertex0 = triangle.Vertex0 * scaleA;
triangle.Vertex1 = triangle.Vertex1 * scaleA;
triangle.Vertex2 = triangle.Vertex2 * scaleA;
// Make AABB test of triangle vs. sweep of B.
if (!GeometryHelper.HaveContact(aabbSweptBInA, triangle.Aabb))
{
continue;
}
triangleShape.Vertex0 = triangle.Vertex0;
triangleShape.Vertex1 = triangle.Vertex1;
triangleShape.Vertex2 = triangle.Vertex2;
float triangleTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
testCollisionObject, targetPoseA,
objectB, targetPoseB,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, triangleTimeOfImpact);
}
}
// Recycle temporary objects.
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
ResourcePools.TriangleShapes.Recycle(triangleShape);
return(timeOfImpact);
}
// Compute contacts between child shapes of two <see cref="CompositeShape"/>s.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildChildContacts(ContactSet contactSet,
int childIndexA,
int childIndexB,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObjectA,
TestGeometricObject testGeometricObjectA,
CollisionObject testCollisionObjectB,
TestGeometricObject testGeometricObjectB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
CompositeShape shapeA = (CompositeShape)geometricObjectA.Shape;
CompositeShape shapeB = (CompositeShape)geometricObjectB.Shape;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
IGeometricObject childA = shapeA.Children[childIndexA];
IGeometricObject childB = shapeB.Children[childIndexB];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, childB];
// ----- Set the shape temporarily to the current children.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
Debug.Assert(
(scaleB.X == scaleB.Y && scaleB.Y == scaleB.Z) || !childB.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childAPose = childA.Pose;
childAPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObjectA.Pose = geometricObjectA.Pose * childAPose;
testGeometricObjectA.Shape = childA.Shape;
testGeometricObjectA.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
var childBPose = childB.Pose;
childBPose.Position *= scaleB; // Apply scaling to local translation.
testGeometricObjectB.Pose = geometricObjectB.Pose * childBPose;
testGeometricObjectB.Shape = childB.Shape;
testGeometricObjectB.Scale = scaleB * childB.Scale; // Apply scaling to local scale.
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// TODO: We could add existing contacts with the same child shape to childContactSet.
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
contact.PositionALocal = childAPose.ToWorldPosition(contact.PositionALocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureA = childIndexA;
//}
contact.PositionBLocal = childBPose.ToWorldPosition(contact.PositionBLocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureB = childIndexB;
//}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
/// <inheritdoc/>
/// <exception cref="ArgumentException">
/// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> is a
/// <see cref="CompositeShape"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
// We get the children and compute the minimal TOI for all children. The child movement is
// linearly interpolated from start to end pose. - This is not correct if the real center
// of rotation and the center of the child shape are not equal! But for small rotational
// movement and small offsets this is acceptable.
// The correct solution:
// Conservative Advancement must not use linear motion interpolation. Instead the correct
// intermediate motion must be computed relative to the parent space.
// The faster solution:
// Use Hierarchical Conservative Advancement as described in the papers by Kim Young et al:
// FAST, C2A, ...
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
// B should be the composite, swap objects if necessary.
if (!(objectB.GeometricObject.Shape is CompositeShape))
{
MathHelper.Swap(ref objectA, ref objectB);
MathHelper.Swap(ref targetPoseA, ref targetPoseB);
}
CompositeShape compositeShapeB = objectB.GeometricObject.Shape as CompositeShape;
// Check if collision objects shapes are correct.
if (compositeShapeB == null)
{
throw new ArgumentException("One object must be a composite shape.");
}
IGeometricObject geometricObjectB = objectB.GeometricObject;
Pose poseB = geometricObjectB.Pose;
Vector3F scaleB = geometricObjectB.Scale;
// Note: Non-uniform scaling for rotated child objects is not supported
// but we might still get a usable TOI query result.
float timeOfImpact = 1;
// Use temporary object.
var testGeometricObjectB = TestGeometricObject.Create();
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
// Go through list of children and find minimal TOI.
int numberOfChildren = compositeShapeB.Children.Count;
for (int i = 0; i < numberOfChildren; i++)
{
IGeometricObject childGeometricObject = compositeShapeB.Children[i];
// Following code is taken from the TransformedShapeAlgorithm:
Pose childPose = childGeometricObject.Pose;
childPose.Position *= scaleB;
testGeometricObjectB.Shape = childGeometricObject.Shape;
testGeometricObjectB.Scale = scaleB * childGeometricObject.Scale;
testGeometricObjectB.Pose = poseB * childPose;
testCollisionObjectB.SetInternal(objectB, testGeometricObjectB);
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[objectA, testCollisionObjectB];
float childTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
objectA, targetPoseA,
testCollisionObjectB, targetPoseB * childPose,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, childTimeOfImpact);
}
// Recycle temporary objects.
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
return(timeOfImpact);
}
public ConstantMedium(IGeometricObject b, double d, Color c)
: this(b, d, new Isotropic(c))
{
}
public ConstantMedium(IGeometricObject b, double d, ITexture a)
: this(b, d, new Isotropic(a))
{
}
public ConstantMedium(IGeometricObject b, double d, Material m)
{
this.boundary = b;
this.negInvDensity = -1.0 / d;
this.phaseFunction = m;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the plane.
// Object B should be the other object.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (convexObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref convexObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
ConvexShape convexShape = convexObject.Shape as ConvexShape;
// Check if shapes are correct.
if (planeShape == null || convexShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a convex shape.", "contactSet");
}
// Get transformations.
Vector3 scalePlane = planeObject.Scale;
Vector3 scaleB = convexObject.Scale;
Pose planePose = planeObject.Pose;
Pose poseB = convexObject.Pose;
// Apply scale to plane and transform plane into world space.
Plane planeWorld = new Plane(planeShape);
planeWorld.Scale(ref scalePlane); // Scale plane.
planeWorld.ToWorld(ref planePose); // Transform plane to world space.
// Transform plane normal to local space of convex.
Vector3 planeNormalLocalB = poseB.ToLocalDirection(planeWorld.Normal);
// Get support vertex nearest to the plane.
Vector3 supportVertexBLocal = convexShape.GetSupportPoint(-planeNormalLocalB, scaleB);
// Transform support vertex into world space.
Vector3 supportVertexBWorld = poseB.ToWorldPosition(supportVertexBLocal);
// Project vertex onto separating axis (given by plane normal).
float distance = Vector3.Dot(supportVertexBWorld, planeWorld.Normal);
// Check for collision.
float penetrationDepth = planeWorld.DistanceFromOrigin - distance;
contactSet.HaveContact = (penetrationDepth >= 0);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
// Position is between support vertex and plane.
Vector3 position = supportVertexBWorld + planeWorld.Normal * (penetrationDepth / 2);
Vector3 normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
if (CollisionDetection.FullContactSetPerFrame &&
type == CollisionQueryType.Contacts &&
contactSet.Count > 0 &&
contactSet.Count < 4)
{
// Special treatment for tetrahedra: Test all vertices against plane.
IList <Vector3> vertices = null;
if (convexShape is ConvexHullOfPoints)
{
var convexHullOfPoints = (ConvexHullOfPoints)convexShape;
vertices = convexHullOfPoints.Points;
}
else if (convexShape is ConvexPolyhedron)
{
var convexPolyhedron = (ConvexPolyhedron)convexShape;
vertices = convexPolyhedron.Vertices;
}
if (vertices != null && vertices.Count <= 8)
{
// Convex has 8 or less vertices. Explicitly test all vertices against the plane.
int numberOfVertices = vertices.Count;
for (int i = 0; i < numberOfVertices; i++)
{
// Test is the same as above.
var vertex = vertices[i];
Vector3 scaledVertex = vertex * scaleB;
if (scaledVertex != supportVertexBLocal) // supportVertexBLocal has already been added.
{
Vector3 vertexWorld = poseB.ToWorldPosition(scaledVertex);
distance = Vector3.Dot(vertexWorld, planeWorld.Normal);
penetrationDepth = planeWorld.DistanceFromOrigin - distance;
if (penetrationDepth >= 0)
{
position = vertexWorld + planeWorld.Normal * (penetrationDepth / 2);
normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal;
contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
}
}
else
{
// Convex is a complex shape with more than 4 vertices.
ContactHelper.TestWithPerturbations(
CollisionDetection,
contactSet,
!swapped, // Perturb the convex object, not the plane.
_computeContactsMethod);
}
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal height field shape algorithm.
_heightFieldAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// HeightField = A, Ray = B
IGeometricObject heightFieldObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the height field, swap objects if necessary.
bool swapped = (heightFieldObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref heightFieldObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
HeightField heightField = heightFieldObject.Shape as HeightField;
// Check if shapes are correct.
if (rayShape == null || heightField == null)
{
throw new ArgumentException("The contact set must contain a ray and a height field.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3 rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3 heightFieldScale = heightFieldObject.Scale;
Pose heightFieldPose = heightFieldObject.Pose;
// We do not support negative scaling. It is not clear what should happen when y is
// scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0)
{
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
}
// Ray in world space.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale);
rayWorld.ToWorld(ref rayPose);
// Ray in local scaled space of the height field.
Ray rayScaled = rayWorld;
rayScaled.ToLocal(ref heightFieldPose);
// Ray in local unscaled space of the mesh.
Ray rayUnscaled = rayScaled;
var inverseCompositeScale = Vector3.One / heightFieldScale;
rayUnscaled.Scale(ref inverseCompositeScale);
// Get height field and basic info.
int arrayLengthX = heightField.NumberOfSamplesX;
int arrayLengthZ = heightField.NumberOfSamplesZ;
int numberOfCellsX = arrayLengthX - 1;
int numberOfCellsZ = arrayLengthZ - 1;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightField.WidthX / numberOfCellsX; // Unscaled!
float cellWidthZ = heightField.WidthZ / numberOfCellsZ; // Unscaled!
// We use a 2D-DDA traversal of the height field cells. In other words: Look at it from
// above. The height field is our screen and we will select the cells as if we draw
// a pixel line. This could be made more efficient when we do not recompute values and
// reuse values and make incremental steps Bresenham-style.
// See GeometryHelper_Casts.cs method HaveContact(Aabb, ray) for explanation of the
// ray parameter formula.
var rayUnscaledDirectionInverse = new Vector3(
1 / rayUnscaled.Direction.X,
1 / rayUnscaled.Direction.Y,
1 / rayUnscaled.Direction.Z);
// The position where the ray enters the current cell.
var cellEnter = rayUnscaled.Origin; // Unscaled!!!
var originX = heightField.OriginX;
var originZ = heightField.OriginZ;
// ----- Find first cell.
int indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; // (int)(...) does not return the desired result for negative values!
if (indexX < 0)
{
if (rayUnscaled.Direction.X <= 0)
{
return;
}
float parameter = (originX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = 0;
}
else if (indexX >= numberOfCellsX)
{
if (rayUnscaled.Direction.X >= 0)
{
return;
}
float parameter = (originX + heightField.WidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = numberOfCellsX - 1;
}
int indexZ = (cellEnter.Z >= originZ) ? (int)((cellEnter.Z - originZ) / cellWidthZ) : -1;
if (indexZ < 0)
{
if (rayUnscaled.Direction.Z <= 0)
{
return;
}
float parameter = (originZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the next height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
// We also have to correct the indexX!
indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
indexZ = 0;
}
else if (indexZ >= numberOfCellsZ)
{
if (rayUnscaled.Direction.Z >= 0)
{
return;
}
float parameter = (originZ + heightField.WidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
if (parameter > rayUnscaled.Length)
{
return; // The ray does not reach the next height field.
}
cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
indexZ = numberOfCellsZ - 1;
}
if (indexX < 0 || indexX >= numberOfCellsX || indexZ < 0 || indexZ >= numberOfCellsZ)
{
return;
}
while (true)
{
// ----- Get triangles of current cell.
var triangle0 = heightField.GetTriangle(indexX, indexZ, false);
var triangle1 = heightField.GetTriangle(indexX, indexZ, true);
// Index of first triangle.
var triangleIndex = (indexZ * numberOfCellsX + indexX) * 2;
float xRelative = (cellEnter.X - originX) / cellWidthX - indexX;
float zRelative = (cellEnter.Z - originZ) / cellWidthZ - indexZ;
bool enterSecondTriangle = (xRelative + zRelative) > 1; // The diagonal is where xRel + zRel == 1.
// ----- Find cell exit and move indices to next cell.
// The position where the ray leaves the current cell.
Vector3 cellExit;
float nextXParameter = float.PositiveInfinity;
if (rayUnscaled.Direction.X > 0)
{
nextXParameter = (originX + (indexX + 1) * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
}
else if (rayUnscaled.Direction.X < 0)
{
nextXParameter = (originX + indexX * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
}
float nextZParameter = float.PositiveInfinity;
if (rayUnscaled.Direction.Z > 0)
{
nextZParameter = (originZ + (indexZ + 1) * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
}
else if (rayUnscaled.Direction.Z < 0)
{
nextZParameter = (originZ + indexZ * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
}
bool isLastCell = false;
if (nextXParameter < nextZParameter)
{
if (rayUnscaled.Direction.X > 0)
{
indexX++;
if (indexX >= numberOfCellsX) // Abort if we have left the height field.
{
isLastCell = true;
}
}
else
{
indexX--;
if (indexX < 0)
{
isLastCell = true;
}
}
if (nextXParameter > rayUnscaled.Length)
{
isLastCell = true; // The ray does not reach the next cell.
nextXParameter = rayUnscaled.Length;
}
cellExit = rayUnscaled.Origin + nextXParameter * rayUnscaled.Direction;
}
else
{
if (rayUnscaled.Direction.Z > 0)
{
indexZ++;
if (indexZ >= numberOfCellsZ)
{
isLastCell = true;
}
}
else
{
indexZ--;
if (indexZ < 0)
{
isLastCell = true;
}
}
if (nextZParameter > rayUnscaled.Length)
{
isLastCell = true;
nextZParameter = rayUnscaled.Length;
}
cellExit = rayUnscaled.Origin + nextZParameter * rayUnscaled.Direction;
}
// ----- We can skip cell if cell AABB is below the ray.
var rayMinY = Math.Min(cellEnter.Y, cellExit.Y) - CollisionDetection.Epsilon; // Apply to avoid missing collisions when ray hits a cell border.
// The ray is above if no height field height is higher the ray height.
// (This check handles NaN height values (holes) correctly.)
bool rayIsAbove = !(triangle0.Vertex0.Y >= rayMinY ||
triangle0.Vertex1.Y >= rayMinY ||
triangle0.Vertex2.Y >= rayMinY ||
triangle1.Vertex1.Y >= rayMinY); // Vertex1 of triangle1 is the fourth quad vertex!
// ----- Test ray against the 2 triangles of the cell.
bool triangle0IsHole = false;
bool triangle1IsHole = false;
if (!rayIsAbove)
{
// Abort if a height value is NaN (hole).
triangle0IsHole = Numeric.IsNaN(triangle0.Vertex0.Y * triangle0.Vertex1.Y * triangle0.Vertex2.Y);
triangle1IsHole = Numeric.IsNaN(triangle1.Vertex0.Y * triangle1.Vertex1.Y * triangle1.Vertex2.Y);
bool contactAdded = false;
if (enterSecondTriangle)
{
// Test second triangle first.
if (!triangle1IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
}
if (!contactAdded && !triangle0IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
}
}
else
{
// Test first triangle first.
if (!triangle0IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
}
if (!contactAdded && !triangle1IsHole)
{
contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
}
}
if (contactAdded)
{
return;
}
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
return;
}
}
// ----- Return simplified contact if cellEnter is below the cell.
if (!rayIsAbove)
{
if (!enterSecondTriangle && !triangle0IsHole && GeometryHelper.IsInFront(triangle0, cellEnter) < 0 ||
enterSecondTriangle && !triangle1IsHole && GeometryHelper.IsInFront(triangle1, cellEnter) < 0)
{
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return;
}
var position = heightFieldPose.ToWorldPosition(cellEnter * heightFieldScale);
var normal = heightFieldPose.ToWorldDirection(Vector3.UnitY);
if (swapped)
{
normal = -normal;
}
float penetrationDepth = (position - rayWorld.Origin).Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return;
}
}
// ----- Move to next cell.
if (isLastCell)
{
return;
}
cellEnter = cellExit;
}
}
// The following methods are used internally by the collision detection to make direct
// changes to a CollisionObject during collision checks.
/// <summary>
/// Copies the data from the specified <see cref="CollisionObject"/> and sets the specified
/// <see cref="IGeometricObject"/>. (For internal use only.)
/// </summary>
/// <param name="collisionObject">The collision object.</param>
/// <param name="geometricObject">The geometric object.</param>
internal void SetInternal(CollisionObject collisionObject, IGeometricObject geometricObject)
{
Changed = collisionObject.Changed;
CollisionGroup = collisionObject.CollisionGroup;
_domain = collisionObject._domain;
Enabled = collisionObject.Enabled;
_geometricObject = geometricObject;
_type = collisionObject._type;
_shapeType = collisionObject._shapeType;
_shape = geometricObject.Shape;
ShapeTypeChanged = collisionObject.ShapeTypeChanged;
}
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 triangle mesh, swap objects if necessary.
// When testing TriangleMeshShape vs. TriangleMeshShape with BVH, object A should be the
// TriangleMeshShape with BVH - except when the TriangleMeshShape with BVH is a lot smaller
// than the other TriangleMeshShape. (See tests below.)
TriangleMeshShape triangleMeshShapeA = geometricObjectA.Shape as TriangleMeshShape;
TriangleMeshShape triangleMeshShapeB = geometricObjectB.Shape as TriangleMeshShape;
bool swapped = false;
// Check if collision objects shapes are correct.
if (triangleMeshShapeA == null && triangleMeshShapeB == null)
{
throw new ArgumentException("The contact set must contain a triangle mesh.", "contactSet");
}
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
Vector3 scaleA = geometricObjectA.Scale;
Vector3 scaleB = geometricObjectB.Scale;
// First we assume that we do not have a contact.
contactSet.HaveContact = false;
// ----- Use temporary test objects.
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();
var testTriangleShapeA = ResourcePools.TriangleShapes.Obtain();
var testTriangleShapeB = ResourcePools.TriangleShapes.Obtain();
try
{
if (triangleMeshShapeA != null &&
triangleMeshShapeA.Partition != null &&
triangleMeshShapeB != null &&
triangleMeshShapeB.Partition != null &&
(type != CollisionQueryType.ClosestPoints ||
triangleMeshShapeA.Partition is ISupportClosestPointQueries <int>))
{
Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine.");
// Find collision algorithm for triangle vs. triangle (used in AddTriangleTriangleContacts()).
if (_triangleTriangleAlgorithm == null)
{
_triangleTriangleAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), typeof(TriangleShape)];
}
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 = triangleMeshShapeB.Partition.GetOverlaps(
scaleB,
geometricObjectB.Pose,
triangleMeshShapeA.Partition,
scaleA,
geometricObjectA.Pose);
}
else
{
overlaps = triangleMeshShapeA.Partition.GetOverlaps(
scaleA,
geometricObjectA.Pose,
triangleMeshShapeB.Partition,
scaleB,
geometricObjectB.Pose);
}
foreach (var overlap in overlaps)
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddTriangleTriangleContacts(
contactSet,
overlapsSwapped ? overlap.Second : overlap.First,
overlapsSwapped ? overlap.First : overlap.Second,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA,
testCollisionObjectB,
testGeometricObjectB,
testTriangleShapeB);
}
}
else
{
// ----- Closest-Point Query
var callback = ClosestPointCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = false;
callback.ContactSet = contactSet;
callback.TestContactSet = testContactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestTriangleA = testTriangleShapeA;
callback.TestTriangleB = testTriangleShapeB;
((ISupportClosestPointQueries <int>)triangleMeshShapeA.Partition)
.GetClosestPointCandidates(
scaleA,
geometricObjectA.Pose,
triangleMeshShapeB.Partition,
scaleB,
geometricObjectB.Pose,
callback.HandlePair);
ClosestPointCallbacks.Recycle(callback);
}
}
else
{
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
// Choose which object should be A.
if (triangleMeshShapeA == null)
{
// A is no TriangleMesh. B must be a TriangleMesh.
swapped = true;
}
else if (triangleMeshShapeB == null)
{
// A is a TriangleMesh and B is no TriangleMesh.
}
else if (triangleMeshShapeA.Partition != null)
{
// A is a TriangleMesh with BVH and B is a TriangleMesh.
// We want to test TriangleMesh with BVH vs. * - unless the TriangleMesh with BVH is a lot
// smaller than the TriangleMesh.
if (largestExtentA * 2 < largestExtentB)
{
swapped = true;
}
}
else if (triangleMeshShapeB.Partition != null)
{
// A is a TriangleMesh and B is a TriangleMesh with BVH.
// We want to test TriangleMesh with BVH vs. * - unless the TriangleMesh BVH is a lot
// smaller than the TriangleMesh.
if (largestExtentB * 2 >= largestExtentA)
{
swapped = true;
}
}
else
{
// A and B are normal triangle meshes. A should be the larger object.
if (largestExtentA < largestExtentB)
{
swapped = true;
}
}
if (swapped)
{
// Swap all variables.
MathHelper.Swap(ref collisionObjectA, ref collisionObjectB);
MathHelper.Swap(ref geometricObjectA, ref geometricObjectB);
MathHelper.Swap(ref aabbOfA, ref aabbOfB);
MathHelper.Swap(ref largestExtentA, ref largestExtentB);
MathHelper.Swap(ref triangleMeshShapeA, ref triangleMeshShapeB);
MathHelper.Swap(ref poseA, ref poseB);
MathHelper.Swap(ref scaleA, ref scaleB);
}
if (triangleMeshShapeB == null &&
type != CollisionQueryType.ClosestPoints &&
largestExtentA * 2 < largestExtentB)
{
// B is a very large object and no triangle mesh.
// Make a AABB vs. Shape of B test for quick rejection.
BoxShape testBoxShape = ResourcePools.BoxShapes.Obtain();
testBoxShape.Extent = aabbOfA.Extent;
testGeometricObjectA.Shape = testBoxShape;
testGeometricObjectA.Scale = Vector3.One;
testGeometricObjectA.Pose = new Pose(aabbOfA.Center);
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, collisionObjectB);
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[testContactSet];
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
ResourcePools.BoxShapes.Recycle(testBoxShape);
if (!testContactSet.HaveContact)
{
contactSet.HaveContact = false;
return;
}
}
if (triangleMeshShapeA.Partition != null &&
(type != CollisionQueryType.ClosestPoints ||
triangleMeshShapeA.Partition is ISupportClosestPointQueries <int>))
{
// Get AABB of B in local space of A.
var aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, poseA.Inverse * poseB);
// Apply inverse scaling to do the AABB-tree checks in the unscaled local space of A.
aabbBInA.Scale(Vector3.One / scaleA);
if (type != CollisionQueryType.ClosestPoints)
{
// Boolean or Contact Query
foreach (var triangleIndex in triangleMeshShapeA.Partition.GetOverlaps(aabbBInA))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddTriangleContacts(
contactSet,
swapped,
triangleIndex,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA);
}
}
else if (type == CollisionQueryType.ClosestPoints)
{
// Closest-Point Query
var callback = ClosestPointCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = swapped;
callback.ContactSet = contactSet;
callback.TestContactSet = testContactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestTriangleA = testTriangleShapeA;
callback.TestTriangleB = testTriangleShapeB;
((ISupportClosestPointQueries <int>)triangleMeshShapeA.Partition)
.GetClosestPointCandidates(
aabbBInA,
float.PositiveInfinity,
callback.HandleItem);
ClosestPointCallbacks.Recycle(callback);
}
}
else
{
// Find an upper bound for the distance we have to search.
// If object are in contact or we make contact/boolean query, then the distance is 0.
float closestPairDistance;
if (contactSet.HaveContact || type != CollisionQueryType.ClosestPoints)
{
closestPairDistance = 0;
}
else
{
// Make first guess for closest pair: inner point of B to inner point of mesh.
Vector3 innerPointA = poseA.ToWorldPosition(geometricObjectA.Shape.InnerPoint * scaleA);
Vector3 innerPointB = poseB.ToWorldPosition(geometricObjectB.Shape.InnerPoint * scaleB);
closestPairDistance = (innerPointB - innerPointA).Length + CollisionDetection.Epsilon;
}
// The search-space is a space where the closest points must lie in.
Vector3 minimum = aabbOfB.Minimum - new Vector3(closestPairDistance);
Vector3 maximum = aabbOfB.Maximum + new Vector3(closestPairDistance);
Aabb searchSpaceAabb = new Aabb(minimum, maximum);
// Test all triangles.
ITriangleMesh triangleMeshA = triangleMeshShapeA.Mesh;
int numberOfTriangles = triangleMeshA.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
// TODO: GetTriangle is performed twice! Here and in AddTriangleContacts() below!
Triangle triangle = triangleMeshA.GetTriangle(i);
testTriangleShapeA.Vertex0 = triangle.Vertex0 * scaleA;
testTriangleShapeA.Vertex1 = triangle.Vertex1 * scaleA;
testTriangleShapeA.Vertex2 = triangle.Vertex2 * scaleA;
// Make AABB test with search space.
if (GeometryHelper.HaveContact(searchSpaceAabb, testTriangleShapeA.GetAabb(poseA)))
{
// IMPORTANT: Info in triangleShape is destroyed in this method!
// Triangle is given to the method so that method does not allocate garbage.
AddTriangleContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
if (closestPairDistance > 0 && contactSet.HaveContact ||
contactSet.Count > 0 && -contactSet[contactSet.Count - 1].PenetrationDepth < closestPairDistance)
{
// Reduce search space
// Note: contactSet can contain several contacts. We assume that the last contact
// is the newest one and check only this.
if (contactSet.Count > 0)
{
closestPairDistance = Math.Max(0, -contactSet[contactSet.Count - 1].PenetrationDepth);
}
else
{
closestPairDistance = 0;
}
searchSpaceAabb.Minimum = aabbOfB.Minimum - new Vector3(closestPairDistance);
searchSpaceAabb.Maximum = aabbOfB.Maximum + new Vector3(closestPairDistance);
}
}
}
}
}
}
finally
{
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
testGeometricObjectA.Recycle();
ResourcePools.TriangleShapes.Recycle(testTriangleShapeB);
ResourcePools.TriangleShapes.Recycle(testTriangleShapeA);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="MinkowskiSumShape"/> class from two geometric
/// objects.
/// </summary>
/// <param name="objectA">The first geometric object.</param>
/// <param name="objectB">The second geometric object.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="GeometryException">
/// <paramref name="objectA"/> is not convex.
/// </exception>
/// <exception cref="GeometryException">
/// <paramref name="objectB"/> is not convex.
/// </exception>
public MinkowskiSumShape(IGeometricObject objectA, IGeometricObject objectB)
{
if (objectA == null)
throw new ArgumentNullException("objectA");
if (objectB == null)
throw new ArgumentNullException("objectB");
_objectA = objectA;
_objectA.PoseChanged += OnChildPoseChanged;
_objectA.ShapeChanged += OnChildShapeChanged;
_objectB = objectB;
_objectB.PoseChanged += OnChildPoseChanged;
_objectB.ShapeChanged += OnChildShapeChanged;
CheckShapes();
}
/// <summary>
/// Estimates the size of an object in pixels.
/// </summary>
/// <param name="cameraNode">The camera node with perspective projection.</param>
/// <param name="viewport">The viewport.</param>
/// <param name="geometricObject">The geometric object.</param>
/// <returns>
/// The estimated width and height of <paramref name="geometricObject"/> in pixels.
/// </returns>
/// <remarks>
/// The method assumes that the object is fully visible by the camera, i.e. it does not perform
/// frustum culling. It estimates the size of <paramref name="geometricObject"/> based on its
/// bounding shape.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="cameraNode"/> or <paramref name="geometricObject"/> is
/// <see langword="null"/>.
/// </exception>
internal static Vector2F GetScreenSize(CameraNode cameraNode, Viewport viewport, IGeometricObject geometricObject)
{
// This implementation is just for reference. (It is preferable to optimize
// and inline the code when needed.)
if (cameraNode == null)
throw new ArgumentNullException("cameraNode");
if (geometricObject == null)
throw new ArgumentNullException("geometricObject");
// Use bounding sphere of AABB in world space.
var aabb = geometricObject.Aabb;
float diameter = aabb.Extent.Length;
float width = diameter;
float height = diameter;
Matrix44F proj = cameraNode.Camera.Projection;
bool isOrthographic = (proj.M33 != 0);
// ----- xScale, yScale:
// Orthographic Projection:
// proj.M00 = 2 / (right - left)
// proj.M11 = 2 / (top - bottom)
//
// Perspective Projection:
// proj.M00 = 2 * zNear / (right - left) = 1 / tan(fovX/2)
// proj.M11 = 2 * zNear / (top - bottom) = 1 / tan(fovY/2)
float xScale = Math.Abs(proj.M00);
float yScale = Math.Abs(proj.M11);
// Screen size [px].
Vector2F screenSize;
if (isOrthographic)
{
// ----- Orthographic Projection
// sizeX = viewportWidth * width / (right - left)
// = viewportWidth * width * xScale / 2
screenSize.X = viewport.Width * width * xScale / 2;
// sizeY = viewportHeight* height / (top - bottom)
// = viewportHeight* height * xScale / 2
screenSize.Y = viewport.Height * height * yScale / 2;
}
else
{
// ----- Perspective Projection
// Camera properties.
Pose cameraPose = cameraNode.PoseWorld;
Vector3F cameraPosition = cameraPose.Position;
Matrix33F cameraOrientation = cameraPose.Orientation;
Vector3F cameraForward = -cameraOrientation.GetColumn(2);
// Get planar distance from camera to object by projecting the distance
// vector onto the look direction.
Vector3F cameraToObject = aabb.Center - cameraPosition;
float distance = Vector3F.Dot(cameraToObject, cameraForward);
// Assume that object is in front of camera (no frustum culling).
distance = Math.Abs(distance);
// Avoid division by zero.
if (distance < Numeric.EpsilonF)
distance = Numeric.EpsilonF;
// sizeX = viewportWidth * width / (objectDistance * 2 * tan(fovX/2))
// = viewportWidth * width * zNear / (objectDistance * (right - left))
// = viewportWidth * width * xScale / (2 * objectDistance)
screenSize.X = viewport.Width * width * xScale / (2 * distance);
// sizeY = viewportHeight * height / (objectDistance * 2 * tan(fovY/2))
// = viewportHeight * height * zNear / (objectDistance * (top - bottom))
// = viewportHeight * height * yScale / (2 * objectDistance)
screenSize.Y = viewport.Height * height * yScale / (2 * distance);
}
return screenSize;
}
private void ComputeCollisionFast(ContactSet contactSet, CollisionQueryType type,
IGeometricObject heightFieldGeometricObject, IGeometricObject convexGeometricObject,
HeightField heightField, ConvexShape convex, bool swapped)
{
Debug.Assert(type != CollisionQueryType.ClosestPoints);
// Assume no contact.
contactSet.HaveContact = false;
// Get scales and poses
Pose heightFieldPose = heightFieldGeometricObject.Pose;
Vector3 heightFieldScale = heightFieldGeometricObject.Scale;
if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0)
{
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
}
Pose convexPose = convexGeometricObject.Pose;
Vector3 convexScale = convexGeometricObject.Scale;
// Get a point in the convex. (Could also use center of AABB.)
var convexPoint = convexPose.ToWorldPosition(convex.InnerPoint * convexScale);
// Get height field coordinates.
convexPoint = heightFieldPose.ToLocalPosition(convexPoint);
float xUnscaled = convexPoint.X / heightFieldScale.X;
float zUnscaled = convexPoint.Z / heightFieldScale.Z;
// If convex point is outside height field, abort.
var originX = heightField.OriginX;
var originZ = heightField.OriginZ;
if (xUnscaled < originX || xUnscaled > originX + heightField.WidthX ||
zUnscaled < originZ || zUnscaled > originZ + heightField.WidthZ)
{
return;
}
// Get height and normal.
float height;
Vector3 normal;
int featureIndex;
GetHeight(heightField, xUnscaled, zUnscaled, out height, out normal, out featureIndex);
// Check for holes.
if (Numeric.IsNaN(height))
{
return;
}
// Apply scaling.
height *= heightFieldScale.Y;
// Normals are transformed with the inverse transposed matrix --> 1 / scale.
normal = normal / heightFieldScale;
normal.Normalize();
// ----- Now we test convex vs. plane.
// Convert normal to convex space.
normal = heightFieldPose.ToWorldDirection(normal);
var normalInConvex = convexPose.ToLocalDirection(normal);
// Convert plane point to convex space.
Vector3 planePoint = new Vector3(convexPoint.X, height, convexPoint.Z);
planePoint = heightFieldPose.ToWorldPosition(planePoint);
planePoint = convexPose.ToLocalPosition(planePoint);
// Get convex support point in plane normal direction.
Vector3 supportPoint = convex.GetSupportPoint(-normalInConvex, convexScale);
// Get penetration depth.
float penetrationDepth = Vector3.Dot((planePoint - supportPoint), normalInConvex);
// Abort if there is no contact.
if (penetrationDepth < 0)
{
return;
}
// Abort if object is too deep under the height field.
// This is important for height fields with holes/caves. Without this check
// no objects could enter the cave.
if (penetrationDepth > heightField.Depth)
{
return;
}
// We have contact.
contactSet.HaveContact = true;
// Return for boolean queries.
if (type == CollisionQueryType.Boolean)
{
return;
}
// Contact position is in the "middle of the penetration".
Vector3 position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2));
if (swapped)
{
normal = -normal;
}
// Add contact
var contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
if (swapped)
{
contact.FeatureB = featureIndex;
}
else
{
contact.FeatureA = featureIndex;
}
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
if (CollisionDetection.FullContactSetPerFrame &&
contactSet.Count < 3)
{
// Trying to create a full contact set.
// We use arbitrary orthonormal values to perturb the normal direction.
var ortho1 = normalInConvex.Orthonormal1;
var ortho2 = normalInConvex.Orthonormal2;
// Test 4 perturbed support directions.
for (int i = 0; i < 4; i++)
{
Vector3 direction;
switch (i)
{
case 0:
direction = -normalInConvex + ortho1;
break;
case 1:
direction = -normalInConvex - ortho1;
break;
case 2:
direction = -normalInConvex + ortho2;
break;
default:
direction = -normalInConvex - ortho2;
break;
}
// Support point vs. plane test as above:
supportPoint = convex.GetSupportPoint(direction, convexScale);
penetrationDepth = Vector3.Dot((planePoint - supportPoint), normalInConvex);
if (penetrationDepth >= 0)
{
position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2));
contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
}
}
/// <summary>
/// Draws a geometric object.
/// </summary>
/// <param name="geometricObject">The geometric object.</param>
/// <param name="color">The color.</param>
/// <param name="drawWireFrame">
/// If set to <see langword="true"/> the wire-frame is drawn; otherwise the mesh is drawn with
/// solid faces.
/// </param>
/// <param name="drawOverScene">
/// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test
/// disabled).
/// </param>
/// <exception cref="NotSupportedException">
/// Drawing solid objects with disabled depth test is not yet supported.
/// </exception>
public void DrawObject(IGeometricObject geometricObject, Color color, bool drawWireFrame, bool drawOverScene)
{
if (geometricObject == null) // Enabled property is checked in DrawShape.
return;
DrawShape(geometricObject.Shape, geometricObject.Pose, geometricObject.Scale, color, drawWireFrame, drawOverScene);
}
//--------------------------------------------------------------
#region Creation and Cleanup
//--------------------------------------------------------------
/// <overloads>
/// <summary>
/// Initializes a new instance of the <see cref="MinkowskiSumShape"/> class.
/// </summary>
/// </overloads>
///
/// <summary>
/// Initializes a new instance of the <see cref="MinkowskiSumShape"/> class.
/// </summary>
public MinkowskiSumShape()
{
_objectA = new GeometricObject(new PointShape());
_objectA.PoseChanged += OnChildPoseChanged;
_objectA.ShapeChanged += OnChildShapeChanged;
_objectB = new GeometricObject(new PointShape());
_objectB.PoseChanged += OnChildPoseChanged;
_objectB.ShapeChanged += OnChildShapeChanged;
}
// Compute contacts between a shape and the child shapes of a <see cref="CompositeShape"/>.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildContacts(ContactSet contactSet,
bool swapped,
int childIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject)
{
// This method is taken from CompositeShapeAlgorithm.cs and slightly modified. Keep changes
// in sync with CompositeShapeAlgorithm.cs!
// !!! Object A should be the composite. - This is different then in ComputeContacts() above!!!
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Vector3 scaleA = geometricObjectA.Scale;
IGeometricObject childA = ((CompositeShape)geometricObjectA.Shape).Children[childIndex];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, geometricObjectB];
// ----- Set the shape temporarily to the current child.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childPose = childA.Pose;
childPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObject.Pose = geometricObjectA.Pose * childPose;
testGeometricObject.Shape = childA.Shape;
testGeometricObject.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Create a temporary contact set.
// (ObjectA and ObjectB should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
{
testContactSet.Reset(collisionObjectB, testCollisionObject);
}
else
{
testContactSet.Reset(testCollisionObject, collisionObjectB);
}
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
if (swapped)
{
contact.PositionBLocal = childPose.ToWorldPosition(contact.PositionBLocal);
contact.FeatureB = childIndex;
}
else
{
contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal);
contact.FeatureA = childIndex;
}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
/// <overloads>
/// <summary>
/// Gets a the bounds of the specified object relative to the viewport.
/// </summary>
/// </overloads>
///
/// <summary>
/// Gets a the bounds of the specified geometric object relative to the viewport.
/// </summary>
/// <param name="cameraNode">The camera node.</param>
/// <param name="geometricObject">The geometric object.</param>
/// <returns>
/// The bounds (left, top, right, bottom) where each entry is in the range [0, 1].
/// </returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="cameraNode"/> or <paramref name="geometricObject"/> is
/// <see langword="null"/>.
/// </exception>
internal static Vector4F GetBounds(CameraNode cameraNode, IGeometricObject geometricObject)
{
// Notes:
// Do not call this GetBounds() method for spheres. Use the other overload for spheres.
//
// At first this problem seems trivial, we only have to get the support
// points of the geometric object's shape in the directions of the frustum
// plane normal vectors. The we project these points to the near plane...
// But this does not work because which can be seen if you draw simple
// drop down sketch. Actually each eye ray has its own direction therefore
// it has its normal and its own support direction!
if (cameraNode == null)
throw new ArgumentNullException("cameraNode");
if (geometricObject == null)
throw new ArgumentNullException("geometricObject");
Debug.Assert(!(geometricObject.Shape is SphereShape), "Call a different GetBounds() overload for spheres!");
// Projection properties.
var camera = cameraNode.Camera;
var projection = camera.Projection;
float near = projection.Near;
float left = projection.Left;
float right = projection.Right;
float width = projection.Width;
float top = projection.Top;
float bottom = projection.Bottom;
float height = projection.Height;
// Get AABB in view space.
Pose localToViewPose = cameraNode.PoseWorld.Inverse * geometricObject.Pose;
Aabb aabb = geometricObject.Shape.GetAabb(geometricObject.Scale, localToViewPose);
// Is the AABB in front of the near plane (= totally clipped)?
if (aabb.Minimum.Z >= -near)
return new Vector4F(0);
// Does the AABB contain the origin?
if (GeometryHelper.HaveContact(aabb, Vector3F.Zero))
return new Vector4F(0, 0, 1, 1);
// Project the AABB far face to the near plane.
Vector2F min;
min.X = aabb.Minimum.X / -aabb.Minimum.Z * near;
min.Y = aabb.Minimum.Y / -aabb.Minimum.Z * near;
Vector2F max;
max.X = aabb.Maximum.X / -aabb.Minimum.Z * near;
max.Y = aabb.Maximum.Y / -aabb.Minimum.Z * near;
// If the AABB z extent overlaps the origin, some results are invalid.
if (aabb.Maximum.Z > -Numeric.EpsilonF)
{
if (aabb.Minimum.X < 0)
min.X = left;
if (aabb.Maximum.X > 0)
max.X = right;
if (aabb.Minimum.Y < 0)
min.Y = bottom;
if (aabb.Maximum.Y > 0)
max.Y = top;
}
else
{
// The AABB near face is also in front. Project AABB near face to near plane
// and take the most extreme.
min.X = Math.Min(min.X, aabb.Minimum.X / -aabb.Maximum.Z * near);
min.Y = Math.Min(min.Y, aabb.Minimum.Y / -aabb.Maximum.Z * near);
max.X = Math.Max(max.X, aabb.Maximum.X / -aabb.Maximum.Z * near);
max.Y = Math.Max(max.Y, aabb.Maximum.Y / -aabb.Maximum.Z * near);
}
Vector4F bounds;
bounds.X = (min.X - left) / width;
bounds.Y = (top - max.Y) / height;
bounds.Z = (max.X - left) / width;
bounds.W = (top - min.Y) / height;
bounds.X = MathHelper.Clamp(bounds.X, 0, 1);
bounds.Y = MathHelper.Clamp(bounds.Y, 0, 1);
bounds.Z = MathHelper.Clamp(bounds.Z, bounds.X, 1);
bounds.W = MathHelper.Clamp(bounds.W, bounds.Y, 1);
return bounds;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal composite shape algorithm.
_compositeAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// Composite = A, Ray = B
IGeometricObject compositeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the composite, swap objects if necessary.
bool swapped = (compositeObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref compositeObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
CompositeShape compositeShape = compositeObject.Shape as CompositeShape;
// Check if shapes are correct.
if (rayShape == null || compositeShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a composite shape.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3 rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3 compositeScale = compositeObject.Scale;
Pose compositePose = compositeObject.Pose;
// Check if transforms are supported.
// Same check for object B.
if (compositeShape != null &&
(compositeScale.X != compositeScale.Y || compositeScale.Y != compositeScale.Z) &&
compositeShape.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 testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
var testGeometricObject = TestGeometricObject.Create();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObject, contactSet.ObjectB); // Dummy arguments! They are changed later.
// Scale ray and transform ray to local unscaled space of composite.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref compositePose); // Transform ray to local space of composite.
var inverseCompositeScale = Vector3.One / compositeScale;
ray.Scale(ref inverseCompositeScale);
try
{
if (compositeShape.Partition != null)
{
foreach (var childIndex in compositeShape.Partition.GetOverlaps(ray))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddChildContacts(
contactSet,
swapped,
childIndex,
type,
testContactSet,
testCollisionObject,
testGeometricObject);
}
}
else
{
var rayDirectionInverse = new Vector3(
1 / ray.Direction.X,
1 / ray.Direction.Y,
1 / ray.Direction.Z);
float epsilon = Numeric.EpsilonF * (1 + compositeObject.Aabb.Extent.Length);
// Go through list of children and find contacts.
int numberOfChildGeometries = compositeShape.Children.Count;
for (int i = 0; i < numberOfChildGeometries; i++)
{
IGeometricObject child = compositeShape.Children[i];
if (GeometryHelper.HaveContact(child.Shape.GetAabb(child.Scale, child.Pose), ray.Origin, rayDirectionInverse, ray.Length, epsilon))
{
AddChildContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObject,
testGeometricObject);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
}
}
}
}
finally
{
Debug.Assert(compositeObject.Shape == compositeShape, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
}
private void ComputeLineVsOther(ContactSet contactSet, CollisionQueryType type, bool objectAIsLine)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Shape shapeA = geometricObjectA.Shape;
Shape shapeB = geometricObjectB.Shape;
Debug.Assert(
shapeA is LineShape && !(shapeB is LineShape) ||
shapeB is LineShape && !(shapeA is LineShape),
"LineAlgorithm.ComputeLineVsOther should only be called for a line and another shape.");
CollisionObject lineCollisionObject;
IGeometricObject lineGeometricObject;
IGeometricObject otherGeometricObject;
LineShape lineShape;
Shape otherShape;
if (objectAIsLine)
{
lineCollisionObject = collisionObjectA;
lineGeometricObject = geometricObjectA;
lineShape = (LineShape)shapeA;
otherGeometricObject = geometricObjectB;
otherShape = shapeB;
}
else
{
lineCollisionObject = collisionObjectB;
lineGeometricObject = geometricObjectB;
lineShape = (LineShape)shapeB;
otherGeometricObject = geometricObjectA;
otherShape = shapeA;
}
// Apply scaling to line.
Line line = new Line(lineShape);
Vector3 lineScale = lineGeometricObject.Scale;
line.Scale(ref lineScale);
// Step 1: Get any bounding sphere that encloses the other object.
Aabb aabb = otherGeometricObject.Aabb;
Vector3 center = (aabb.Minimum + aabb.Maximum) / 2;
float radius = (aabb.Maximum - aabb.Minimum).Length; // A large safe radius. (Exact size does not matter.)
// Step 2: Get the closest point of line vs. center.
// All computations in local space of the line.
Vector3 closestPointOnLine;
Pose linePose = lineGeometricObject.Pose;
GeometryHelper.GetClosestPoint(line, linePose.ToLocalPosition(center), out closestPointOnLine);
// Step 3: Crop the line to a line segment that will contain the closest point.
var lineSegment = ResourcePools.LineSegmentShapes.Obtain();
lineSegment.Start = closestPointOnLine - line.Direction * radius;
lineSegment.End = closestPointOnLine + line.Direction * radius;
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = lineSegment;
testGeometricObject.Scale = Vector3.One;
testGeometricObject.Pose = linePose;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(lineCollisionObject, testGeometricObject);
var testContactSet = objectAIsLine ? ContactSet.Create(testCollisionObject, collisionObjectB)
: ContactSet.Create(collisionObjectA, testCollisionObject);
testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;
// Step 4: Call another collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[lineSegment, otherShape];
// Step 5: Manually chosen preferred direction for MPR.
// For the MPR we choose the best ray direction ourselves. The ray should be normal
// to the line, otherwise MPR could try to push the line segment out of the other object
// in the line direction - this cannot work for infinite lines.
// Results without a manual MPR ray were ok for normal cases. Problems were only observed
// for cases where the InnerPoints overlap or for deep interpenetrations.
Vector3 v0A = geometricObjectA.Pose.ToWorldPosition(shapeA.InnerPoint * geometricObjectA.Scale);
Vector3 v0B = geometricObjectB.Pose.ToWorldPosition(shapeB.InnerPoint * geometricObjectB.Scale);
Vector3 n = v0B - v0A; // This is the default MPR ray direction.
// Make n normal to the line.
n = n - Vector3.ProjectTo(n, linePose.ToWorldDirection(lineShape.Direction));
if (!n.TryNormalize())
{
n = lineShape.Direction.Orthonormal1;
}
testContactSet.PreferredNormal = n;
collisionAlgorithm.ComputeCollision(testContactSet, type);
if (testContactSet.HaveContact)
{
contactSet.HaveContact = true;
}
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
ResourcePools.LineSegmentShapes.Recycle(lineSegment);
}
/// <summary>
/// Computes the collision between line vs. line.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="type">The type of collision query.</param>
private void ComputeLineVsLine(ContactSet contactSet, CollisionQueryType type)
{
IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
Debug.Assert(objectA.Shape is LineShape && objectB.Shape is LineShape, "LineAlgorithm.ComputeLineVsLine should only be called for 2 line shapes.");
Debug.Assert(contactSet.Count <= 1, "Two lines should have at max 1 contact point.");
// Get transformations.
Vector3 scaleA = objectA.Scale;
Vector3 scaleB = objectB.Scale;
Pose poseA = objectA.Pose;
Pose poseB = objectB.Pose;
// Create two line objects in world space.
var lineA = new Line((LineShape)objectA.Shape);
lineA.Scale(ref scaleA);
lineA.ToWorld(ref poseA);
var lineB = new Line((LineShape)objectB.Shape);
lineB.Scale(ref scaleB);
lineB.ToWorld(ref poseB);
// Get closest points.
Vector3 pointA;
Vector3 pointB;
contactSet.HaveContact = GeometryHelper.GetClosestPoints(lineA, lineB, out pointA, out pointB);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
// Create contact information.
Vector3 position = (pointA + pointB) / 2;
Vector3 normal = pointB - pointA;
float length = normal.Length;
if (Numeric.IsZero(length))
{
// Create normal from cross product of both lines.
normal = Vector3.Cross(lineA.Direction, lineB.Direction);
if (!normal.TryNormalize())
{
normal = Vector3.UnitY;
}
}
else
{
// Normalize vector
normal = normal / length;
}
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -length, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
public CollisionAlgorithm this[IGeometricObject geometricObjectA, IGeometricObject geometricObjectB]
{
get
{
if (geometricObjectA == null)
throw new ArgumentNullException("geometricObjectA");
if (geometricObjectB == null)
throw new ArgumentNullException("geometricObjectB");
Debug.Assert(geometricObjectA.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
Debug.Assert(geometricObjectB.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
return this[geometricObjectA.Shape.GetType(), geometricObjectB.Shape.GetType()];
}
set
{
if (geometricObjectA == null)
throw new ArgumentNullException("geometricObjectA");
if (geometricObjectB == null)
throw new ArgumentNullException("geometricObjectB");
Debug.Assert(geometricObjectA.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
Debug.Assert(geometricObjectB.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
this[geometricObjectA.Shape.GetType(), geometricObjectB.Shape.GetType()] = value;
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the plane.
// Object B should be the other object.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject boxObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (boxObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref boxObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
BoxShape boxShape = boxObject.Shape as BoxShape;
// Check if shapes are correct.
if (planeShape == null || boxShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a box shape.", "contactSet");
}
// Get transformations.
Vector3F scalePlane = planeObject.Scale;
Vector3F scaleBox = boxObject.Scale;
Pose posePlane = planeObject.Pose;
Pose poseBox = boxObject.Pose;
// Apply scale to plane and transform plane into world space.
Plane planeWorld = new Plane(planeShape);
planeWorld.Scale(ref scalePlane); // Scale plane.
planeWorld.ToWorld(ref posePlane); // Transform plane to world space.
// Transform plane normal to local space of box.
Vector3F planeNormalLocalB = poseBox.ToLocalDirection(planeWorld.Normal);
// Get support vertex nearest to the plane.
Vector3F supportVertexBLocal = boxShape.GetSupportPoint(-planeNormalLocalB, scaleBox);
// Transform support vertex into world space.
Vector3F supportVertexBWorld = poseBox.ToWorldPosition(supportVertexBLocal);
// Project vertex onto separating axis (given by plane normal).
float distance = Vector3F.Dot(supportVertexBWorld, planeWorld.Normal);
// Check for collision.
float penetrationDepth = planeWorld.DistanceFromOrigin - distance;
contactSet.HaveContact = (penetrationDepth >= 0);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
// We have contact.
if (!CollisionDetection.FullContactSetPerFrame || type == CollisionQueryType.ClosestPoints)
{
// Use only the already known contact.
AddContact(ref supportVertexBWorld, ref planeWorld, penetrationDepth, swapped, contactSet, type);
}
else
{
// Apply scale to box extent.
Vector3F boxHalfExtent = boxShape.Extent * scaleBox * 0.5f;
// Check all box vertices against plane.
CheckContact(ref planeWorld, new Vector3F(-boxHalfExtent.X, -boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(-boxHalfExtent.X, -boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(-boxHalfExtent.X, boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(-boxHalfExtent.X, boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(boxHalfExtent.X, -boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(boxHalfExtent.X, -boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(boxHalfExtent.X, boxHalfExtent.Y, -boxHalfExtent.Z), ref poseBox, swapped, contactSet);
CheckContact(ref planeWorld, new Vector3F(boxHalfExtent.X, boxHalfExtent.Y, boxHalfExtent.Z), ref poseBox, swapped, contactSet);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="TransformedShape"/> class from the given
/// geometric object.
/// </summary>
/// <param name="child">The geometric object (pose + shape).</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="child"/> is <see langword="null"/>.
/// </exception>
public TransformedShape(IGeometricObject child)
{
if (child == null)
throw new ArgumentNullException("child");
_child = child;
_child.PoseChanged += OnChildPoseChanged;
_child.ShapeChanged += OnChildShapeChanged;
}
//--------------------------------------------------------------
#region Creation and Cleanup
//--------------------------------------------------------------
/// <overloads>
/// <summary>
/// Initializes a new instance of the <see cref="TransformedShape"/> class.
/// </summary>
/// </overloads>
///
/// <summary>
/// Initializes a new instance of the <see cref="TransformedShape"/> class.
/// </summary>
/// <remarks>
/// <see cref="Child"/> is initialized with a <see cref="Child"/>
/// with an <see cref="EmptyShape"/>.
/// </remarks>
public TransformedShape()
{
_child = new GeometricObject(Empty);
_child.PoseChanged += OnChildPoseChanged;
_child.ShapeChanged += OnChildShapeChanged;
}
