public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { if (type == CollisionQueryType.ClosestPoints) { // Closest point queries. _closestPointsAlgorithm.ComputeCollision(contactSet, type); if (contactSet.HaveContact) { // Penetration. // Remember the closest point info in case we run into troubles. // Get the last contact. We assume that this is the newest. In most cases there will // only be one contact in the contact set. Contact fallbackContact = (contactSet.Count > 0) ? contactSet[contactSet.Count - 1] : null; // Call the contact query algorithm. _contactAlgorithm.ComputeCollision(contactSet, CollisionQueryType.Contacts); if (!contactSet.HaveContact) { // Problem! // The closest-point algorithm reported contact. The contact algorithm didn't find a contact. // This can happen, for example, because of numerical inaccuracies in GJK vs. MPR. // Keep the result of the closest-point computation, but decrease the penetration depth // to indicate separation. if (fallbackContact != null) { Debug.Assert(fallbackContact.PenetrationDepth == 0); fallbackContact.PenetrationDepth = -Math.Min(10 * Numeric.EpsilonF, CollisionDetection.Epsilon); foreach (var contact in contactSet) { if (contact != fallbackContact) { contact.Recycle(); } } contactSet.Clear(); contactSet.Add(fallbackContact); } contactSet.HaveContact = false; } } } else { // Boolean or contact queries. _contactAlgorithm.ComputeCollision(contactSet, type); } }
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); Vector3F lineScale = lineGeometricObject.Scale; line.Scale(ref lineScale); // Step 1: Get any bounding sphere that encloses the other object. Aabb aabb = otherGeometricObject.Aabb; Vector3F 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. Vector3F 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 = Vector3F.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. Vector3F v0A = geometricObjectA.Pose.ToWorldPosition(shapeA.InnerPoint * geometricObjectA.Scale); Vector3F v0B = geometricObjectB.Pose.ToWorldPosition(shapeB.InnerPoint * geometricObjectB.Scale); Vector3F n = v0B - v0A; // This is the default MPR ray direction. // Make n normal to the line. n = n - Vector3F.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); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Object A should be the plane. // Object B should be the sphere. IGeometricObject planeObject = contactSet.ObjectA.GeometricObject; IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject; // A should be the plane, swap objects if necessary. bool swapped = (sphereObject.Shape is PlaneShape); if (swapped) { MathHelper.Swap(ref planeObject, ref sphereObject); } PlaneShape planeShape = planeObject.Shape as PlaneShape; SphereShape sphereShape = sphereObject.Shape as SphereShape; // Check if collision object shapes are correct. if (planeShape == null || sphereShape == null) { throw new ArgumentException("The contact set must contain a plane and a sphere.", "contactSet"); } // Get scalings. Vector3F planeScale = planeObject.Scale; Vector3F sphereScale = Vector3F.Absolute(sphereObject.Scale); // Call other algorithm for non-uniformly scaled spheres. if (sphereScale.X != sphereScale.Y || sphereScale.Y != sphereScale.Z) { if (_fallbackAlgorithm == null) { _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(PlaneShape), typeof(ConvexShape)]; } _fallbackAlgorithm.ComputeCollision(contactSet, type); return; } // Get poses. Pose planePose = planeObject.Pose; Pose spherePose = sphereObject.Pose; // Apply scaling to plane and transform plane to world space. Plane plane = new Plane(planeShape); plane.Scale(ref planeScale); // Scale plane. plane.ToWorld(ref planePose); // Transform plane to world space. // Calculate distance from plane to sphere surface. float sphereRadius = sphereShape.Radius * sphereScale.X; Vector3F sphereCenter = spherePose.Position; float planeToSphereDistance = Vector3F.Dot(sphereCenter, plane.Normal) - sphereRadius - plane.DistanceFromOrigin; float penetrationDepth = -planeToSphereDistance; 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; } // Compute contact details. Vector3F position = sphereCenter - plane.Normal * (sphereRadius - penetrationDepth / 2); Vector3F normal = (swapped) ? -plane.Normal : plane.Normal; // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
// 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; Vector3F 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); } }
// The parameters 'testXxx' are initialized objects which are re-used to avoid a lot of GC garbage. private void AddTriangleTriangleContacts( ContactSet contactSet, int triangleIndexA, int triangleIndexB, CollisionQueryType type, ContactSet testContactSet, CollisionObject testCollisionObjectA, TestGeometricObject testGeometricObjectA, TriangleShape testTriangleA, CollisionObject testCollisionObjectB, TestGeometricObject testGeometricObjectB, TriangleShape testTriangleB) { CollisionObject collisionObjectA = contactSet.ObjectA; CollisionObject collisionObjectB = contactSet.ObjectB; IGeometricObject geometricObjectA = collisionObjectA.GeometricObject; IGeometricObject geometricObjectB = collisionObjectB.GeometricObject; TriangleMeshShape triangleMeshShapeA = (TriangleMeshShape)geometricObjectA.Shape; Triangle triangleA = triangleMeshShapeA.Mesh.GetTriangle(triangleIndexA); TriangleMeshShape triangleMeshShapeB = (TriangleMeshShape)geometricObjectB.Shape; Triangle triangleB = triangleMeshShapeB.Mesh.GetTriangle(triangleIndexB); Pose poseA = geometricObjectA.Pose; Pose poseB = geometricObjectB.Pose; Vector3F scaleA = geometricObjectA.Scale; Vector3F scaleB = geometricObjectB.Scale; // Apply SRT. Triangle transformedTriangleA; transformedTriangleA.Vertex0 = poseA.ToWorldPosition(triangleA.Vertex0 * scaleA); transformedTriangleA.Vertex1 = poseA.ToWorldPosition(triangleA.Vertex1 * scaleA); transformedTriangleA.Vertex2 = poseA.ToWorldPosition(triangleA.Vertex2 * scaleA); Triangle transformedTriangleB; transformedTriangleB.Vertex0 = poseB.ToWorldPosition(triangleB.Vertex0 * scaleB); transformedTriangleB.Vertex1 = poseB.ToWorldPosition(triangleB.Vertex1 * scaleB); transformedTriangleB.Vertex2 = poseB.ToWorldPosition(triangleB.Vertex2 * scaleB); // Make super-fast boolean check first. This is redundant if we have to compute // a contact with SAT below. But in stochastic benchmarks it seems to be 10% faster. bool haveContact = GeometryHelper.HaveContact(ref transformedTriangleA, ref transformedTriangleB); if (type == CollisionQueryType.Boolean) { contactSet.HaveContact = (contactSet.HaveContact || haveContact); return; } if (haveContact) { // Make sure the scaled triangles have the correct normal. // (A negative scale changes the normal/winding order. See unit test in TriangleTest.cs.) if (scaleA.X * scaleA.Y * scaleA.Z < 0) { MathHelper.Swap(ref transformedTriangleA.Vertex0, ref transformedTriangleA.Vertex1); } if (scaleB.X * scaleB.Y * scaleB.Z < 0) { MathHelper.Swap(ref transformedTriangleB.Vertex0, ref transformedTriangleB.Vertex1); } // Compute contact. Vector3F position, normal; float penetrationDepth; haveContact = TriangleTriangleAlgorithm.GetContact( ref transformedTriangleA, ref transformedTriangleB, !triangleMeshShapeA.IsTwoSided, !triangleMeshShapeB.IsTwoSided, out position, out normal, out penetrationDepth); if (haveContact) { contactSet.HaveContact = true; // In deep interpenetrations we might get no contact (penDepth = NaN). if (!Numeric.IsNaN(penetrationDepth)) { Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); contact.FeatureA = triangleIndexA; contact.FeatureB = triangleIndexB; ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } return; } // We might come here if the boolean test reports contact but the SAT test // does not because of numerical errors. } Debug.Assert(!haveContact); if (type == CollisionQueryType.Contacts) { return; } Debug.Assert(type == CollisionQueryType.ClosestPoints); if (contactSet.HaveContact) { // These triangles are separated but other parts of the meshes touches. // --> Abort. return; } // We do not have a specialized triangle-triangle closest points algorithm. // Fall back to the default algorithm (GJK). // Initialize temporary test contact set and test objects. // Note: We assume the triangle-triangle does not care about front/back faces. testTriangleA.Vertex0 = transformedTriangleA.Vertex0; testTriangleA.Vertex1 = transformedTriangleA.Vertex1; testTriangleA.Vertex2 = transformedTriangleA.Vertex2; testGeometricObjectA.Shape = testTriangleA; Debug.Assert(testGeometricObjectA.Scale == Vector3F.One); Debug.Assert(testGeometricObjectA.Pose == Pose.Identity); testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA); testTriangleB.Vertex0 = transformedTriangleB.Vertex0; testTriangleB.Vertex1 = transformedTriangleB.Vertex1; testTriangleB.Vertex2 = transformedTriangleB.Vertex2; testGeometricObjectB.Shape = testTriangleB; Debug.Assert(testGeometricObjectB.Scale == Vector3F.One); Debug.Assert(testGeometricObjectB.Pose == Pose.Identity); testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB); Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared."); testContactSet.Reset(testCollisionObjectA, testCollisionObjectB); testContactSet.IsPerturbationTestAllowed = false; _triangleTriangleAlgorithm.ComputeCollision(testContactSet, type); // Note: We expect no contact but because of numerical differences the triangle-triangle // algorithm could find a shallow surface contact. contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact); #region ----- Merge testContactSet into contactSet ----- 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. //{ contact.FeatureA = triangleIndexA; contact.FeatureB = triangleIndexB; //} } // Merge the contact info. ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance); } #endregion }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { IGeometricObject sphereObjectA = contactSet.ObjectA.GeometricObject; IGeometricObject sphereObjectB = contactSet.ObjectB.GeometricObject; SphereShape sphereShapeA = sphereObjectA.Shape as SphereShape; SphereShape sphereShapeB = sphereObjectB.Shape as SphereShape; // Check if collision objects are spheres. if (sphereShapeA == null || sphereShapeB == null) { throw new ArgumentException("The contact set must contain sphere shapes.", "contactSet"); } Vector3F scaleA = Vector3F.Absolute(sphereObjectA.Scale); Vector3F scaleB = Vector3F.Absolute(sphereObjectB.Scale); // Call MPR for non-uniformly scaled spheres. if (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z || scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z) { if (_fallbackAlgorithm == null) { _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), typeof(ConvexShape)]; } _fallbackAlgorithm.ComputeCollision(contactSet, type); return; } // Apply uniform scale. float radiusA = sphereShapeA.Radius * scaleA.X; float radiusB = sphereShapeB.Radius * scaleB.X; // Vector from center of A to center of B. Vector3F centerA = sphereObjectA.Pose.Position; Vector3F centerB = sphereObjectB.Pose.Position; Vector3F aToB = centerB - centerA; float lengthAToB = aToB.Length; // Check radius of spheres. float penetrationDepth = radiusA + radiusB - lengthAToB; 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; } // ----- Create contact information. Vector3F normal; if (Numeric.IsZero(lengthAToB)) { // Spheres are on the same position, we can choose any normal vector. // Possibly it would be better to consider the object movement (velocities), but // it is not important since this case should be VERY rare. normal = Vector3F.UnitY; } else { normal = aToB.Normalized; } // The contact point lies in the middle of the intersecting volume. Vector3F position = centerA + normal * (radiusA - penetrationDepth / 2); // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Object A should be the height field. CollisionObject heightFieldCollisionObject = contactSet.ObjectA; CollisionObject otherCollisionObject = contactSet.ObjectB; // Swap objects if necessary. bool swapped = !(heightFieldCollisionObject.GeometricObject.Shape is HeightField); if (swapped) { MathHelper.Swap(ref heightFieldCollisionObject, ref otherCollisionObject); } IGeometricObject heightFieldGeometricObject = heightFieldCollisionObject.GeometricObject; IGeometricObject otherGeometricObject = otherCollisionObject.GeometricObject; HeightField heightField = heightFieldGeometricObject.Shape as HeightField; Shape otherShape = otherGeometricObject.Shape; // Check if collision object shapes are correct. if (heightField == null) { throw new ArgumentException("The contact set must contain a height field.", "contactSet"); } if (heightField.UseFastCollisionApproximation && type != CollisionQueryType.ClosestPoints) { // If other object is convex, use the new fast collision detection algorithm. ConvexShape convex = otherShape as ConvexShape; if (convex != null) { ComputeCollisionFast( contactSet, type, heightFieldGeometricObject, otherGeometricObject, heightField, convex, swapped); return; } } #region ----- Precomputations ----- Vector3F scaleHeightField = heightFieldGeometricObject.Scale; Vector3F scaleOther = otherGeometricObject.Scale; Pose heightFieldPose = heightFieldGeometricObject.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 (scaleHeightField.X < 0 || scaleHeightField.Y < 0 || scaleHeightField.Z < 0) { throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported."); } // Get height field and basic info. Vector3F heightFieldUpAxis = heightFieldPose.ToWorldDirection(Vector3F.UnitY); int arrayLengthX = heightField.NumberOfSamplesX; int arrayLengthZ = heightField.NumberOfSamplesZ; Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell)."); float cellWidthX = heightField.WidthX * scaleHeightField.X / (arrayLengthX - 1); float cellWidthZ = heightField.WidthZ * scaleHeightField.Z / (arrayLengthZ - 1); // The search-space is the rectangular region on the height field where the closest points // must lie in. For contacts we do not have to search neighbor cells. For closest-point // queries and separation we have to search neighbor cells. // We compute the search-space using a current maximum search distance. float currentSearchDistance = 0; Contact guessedClosestPair = null; if (!contactSet.HaveContact && type == CollisionQueryType.ClosestPoints) { // Make a guess for the closest pair using SupportMapping or InnerPoints. bool isOverHole; guessedClosestPair = GuessClosestPair(contactSet, swapped, out isOverHole); if (isOverHole) { // Guesses over holes are useless. --> Check the whole terrain. currentSearchDistance = heightFieldGeometricObject.Aabb.Extent.Length; } else if (guessedClosestPair.PenetrationDepth < 0) { currentSearchDistance = -guessedClosestPair.PenetrationDepth; } else { contactSet.HaveContact = true; } } else { // Assume no contact. contactSet.HaveContact = false; } // Get AABB of the other object in local space of the height field. Aabb aabbOfOther = otherShape.GetAabb(scaleOther, heightFieldPose.Inverse * otherGeometricObject.Pose); float originX = heightField.OriginX * scaleHeightField.X; float originZ = heightField.OriginZ * scaleHeightField.Z; // ----- Compute the cell indices of the search-space. // Estimate start and end indices from our search distance. int xIndexStartEstimated = (int)((aabbOfOther.Minimum.X - currentSearchDistance - originX) / cellWidthX); int xIndexEndEstimated = (int)((aabbOfOther.Maximum.X + currentSearchDistance - originX) / cellWidthX); int zIndexStartEstimated = (int)((aabbOfOther.Minimum.Z - currentSearchDistance - originZ) / cellWidthZ); int zIndexEndEstimated = (int)((aabbOfOther.Maximum.Z + currentSearchDistance - originZ) / cellWidthZ); // Clamp indices to valid range. int xIndexMax = arrayLengthX - 2; int zIndexMax = arrayLengthZ - 2; int xIndexStart = Math.Max(xIndexStartEstimated, 0); int xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax); int zIndexStart = Math.Max(zIndexStartEstimated, 0); int zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax); // Find collision algorithm for MinkowskiSum vs. other object's shape. CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), otherShape.GetType()]; int numberOfContactsInLastFrame = contactSet.Count; #endregion #region ----- Test all height field cells in the search space. ----- // Create several temporary test objects: // Instead of the original height field geometric object, we test against a shape for each // height field triangle. For the test shape we "extrude" the triangle under the height field. // To create the extrusion we "add" a line segment to the triangle using a Minkowski sum. // TODO: We can make this faster with a special shape that knows that the child poses are Identity (instead of the standard MinkowskiSumShape). // This special shape could compute its InnerPoint without applying the poses. var triangleShape = ResourcePools.TriangleShapes.Obtain(); // (Vertices will be set in the loop below.) var triangleGeometricObject = TestGeometricObject.Create(); triangleGeometricObject.Shape = triangleShape; var lineSegment = ResourcePools.LineSegmentShapes.Obtain(); lineSegment.Start = Vector3F.Zero; lineSegment.End = -heightField.Depth * Vector3F.UnitY; var lineSegmentGeometricObject = TestGeometricObject.Create(); lineSegmentGeometricObject.Shape = lineSegment; var extrudedTriangleShape = TestMinkowskiSumShape.Create(); extrudedTriangleShape.ObjectA = triangleGeometricObject; extrudedTriangleShape.ObjectB = lineSegmentGeometricObject; var extrudedTriangleGeometricObject = TestGeometricObject.Create(); extrudedTriangleGeometricObject.Shape = extrudedTriangleShape; extrudedTriangleGeometricObject.Pose = heightFieldPose; var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain(); testCollisionObject.SetInternal(heightFieldCollisionObject, extrudedTriangleGeometricObject); var testContactSet = swapped ? ContactSet.Create(contactSet.ObjectA, testCollisionObject) : ContactSet.Create(testCollisionObject, contactSet.ObjectB); testContactSet.IsPerturbationTestAllowed = false; // We compute closest points with a preferred normal direction: the height field up-axis. // Loop over the cells in the search space. // (The inner loop can reduce the search space. Therefore, when we increment the indices // xIndex and zIndex we also check if the start indices have changed.) for (int xIndex = xIndexStart; xIndex <= xIndexEnd; xIndex = Math.Max(xIndexStart, xIndex + 1)) { for (int zIndex = zIndexStart; zIndex <= zIndexEnd; zIndex = Math.Max(zIndexStart, zIndex + 1)) { // Test the two cell triangles. for (int triangleIndex = 0; triangleIndex < 2; triangleIndex++) { // Get triangle 0 or 1. var triangle = heightField.GetTriangle(xIndex, zIndex, triangleIndex != 0); var triangleIsHole = Numeric.IsNaN(triangle.Vertex0.Y * triangle.Vertex1.Y * triangle.Vertex2.Y); if (triangleIsHole) { continue; } triangleShape.Vertex0 = triangle.Vertex0 * scaleHeightField; triangleShape.Vertex1 = triangle.Vertex1 * scaleHeightField; triangleShape.Vertex2 = triangle.Vertex2 * scaleHeightField; if (type == CollisionQueryType.Boolean) { collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean); contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact; if (contactSet.HaveContact) { // We can stop tests here for boolean queries. // Update end indices to exit the outer loops. xIndexEnd = -1; zIndexEnd = -1; break; } } else { Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared."); // 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); if (testContactSet.HaveContact) { contactSet.HaveContact = true; if (testContactSet.Count > 0) { // Get neighbor triangle. // To compute the triangle normal we take the normal of the unscaled triangle and transform // the normal with: (M^-1)^T = 1 / scale // Note: We cannot use the scaled vertices because negative scalings change the // face-order of the vertices. Vector3F triangleNormal = triangle.Normal / scaleHeightField; triangleNormal = heightFieldPose.ToWorldDirection(triangleNormal); triangleNormal.TryNormalize(); // Assuming the last contact is the newest. (With closest-point queries // and the CombinedCollisionAlgo, testContactSet[0] could be a (not so useful) // closest-point result, and testContactSet[1] the better contact query result.) var testContact = testContactSet[testContactSet.Count - 1]; var contactNormal = swapped ? -testContact.Normal : testContact.Normal; if (Vector3F.Dot(contactNormal, triangleNormal) < WeldingLimit) { // Contact normal deviates by more than the welding limit. --> Check the contact. // If we do not find a neighbor, we assume the neighbor has the same normal. var neighborNormal = triangleNormal; #region ----- Get Neighbor Triangle Normal ----- // Get barycentric coordinates of contact position. Vector3F contactPositionOnHeightField = swapped ? testContact.PositionBLocal / scaleHeightField : testContact.PositionALocal / scaleHeightField; float u, v, w; // TODO: GetBaryCentricFromPoint computes the triangle normal, which we already know - optimize. GeometryHelper.GetBarycentricFromPoint(triangle, contactPositionOnHeightField, out u, out v, out w); // If one coordinate is near 0, the contact is near an edge. if (u < 0.05f || v < 0.05f || w < 0.05f) { if (triangleIndex == 0) { if (u < v && u < w) { neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex, true).Normal / scaleHeightField); neighborNormal.TryNormalize(); } else if (v < w) { if (zIndex > 0) { neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex - 1, true).Normal / scaleHeightField); neighborNormal.TryNormalize(); } else { // The contact is at the border of the whole height field. Set a normal which disables all bad contact filtering. neighborNormal = new Vector3F(float.NaN); } } else { if (xIndex > 0) { neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex - 1, zIndex, true).Normal / scaleHeightField); neighborNormal.TryNormalize(); } else { neighborNormal = new Vector3F(float.NaN); } } } else { if (u < v && u < w) { if (xIndex + 2 < arrayLengthX) { neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex + 1, zIndex, false).Normal / scaleHeightField); neighborNormal.TryNormalize(); } else { neighborNormal = new Vector3F(float.NaN); } } else if (v < w) { neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex, false).Normal / scaleHeightField); neighborNormal.TryNormalize(); } else { if (zIndex + 2 < arrayLengthZ) { neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex + 1, true).Normal / scaleHeightField); neighborNormal.TryNormalize(); } else { neighborNormal = new Vector3F(float.NaN); } } } } #endregion // Contact normals in the range triangleNormal - neighborNormal are allowed. // Others, especially vertical contacts in slopes or horizontal normals are not // allowed. var cosMinAngle = Vector3F.Dot(neighborNormal, triangleNormal) - CollisionDetection.Epsilon; RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle); // If we have no contact yet, we retry with a preferred normal identical to the up axis. // (Note: contactSet.Count will be > 0 for closest-point queries but will // probably constraint separated contacts and not real contacts.) if (testContactSet.Count == 0 && (contactSet.Count == 0 || type == CollisionQueryType.ClosestPoints)) { testContactSet.PreferredNormal = (swapped) ? -heightFieldUpAxis : heightFieldUpAxis; collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts); testContactSet.PreferredNormal = Vector3F.Zero; RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle); } // If we have no contact yet, we retry with a preferred normal identical to the triangle normal. // But only if the triangle normal differs significantly from the up axis. if (testContactSet.Count == 0 && (contactSet.Count == 0 || type == CollisionQueryType.ClosestPoints) && Vector3F.Dot(heightFieldUpAxis, triangleNormal) < WeldingLimit) { testContactSet.PreferredNormal = (swapped) ? -triangleNormal : triangleNormal; collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts); testContactSet.PreferredNormal = Vector3F.Zero; RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle); } } } } if (testContactSet.Count > 0) { // Remember separation distance for later. float separationDistance = -testContactSet[0].PenetrationDepth; // Set the shape feature of the new contacts. // The features is the height field triangle index (see HeightField documentation). int numberOfContacts = testContactSet.Count; for (int i = 0; i < numberOfContacts; i++) { Contact contact = testContactSet[i]; int featureIndex = (zIndex * (arrayLengthX - 1) + xIndex) * 2 + triangleIndex; if (swapped) { contact.FeatureB = featureIndex; } else { contact.FeatureA = featureIndex; } } // Merge the contact info. (Contacts in testContactSet are recycled!) ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance); #region ----- Update search space ----- // Update search space if possible. // The best search distance is 0. For separation we can use the current smallest // separation as search distance. As soon as we have a contact, we set the // search distance to 0. if (currentSearchDistance > 0 && // No need to update search space if search distance is already 0. (contactSet.HaveContact || // If we have a contact, we set the search distance to 0. separationDistance < currentSearchDistance)) // If we have closer separation, we use this. { // Note: We only check triangleContactSet[0] in the if condition. // triangleContactSet could contain several contacts, but we don't bother with // this special case. // Update search distance. if (contactSet.HaveContact) { currentSearchDistance = 0; } else { currentSearchDistance = Math.Max(0, separationDistance); } // Update search space indices. xIndexStartEstimated = (int)((aabbOfOther.Minimum.X - currentSearchDistance - originX) / cellWidthX); xIndexEndEstimated = (int)((aabbOfOther.Maximum.X + currentSearchDistance - originX) / cellWidthX); zIndexStartEstimated = (int)((aabbOfOther.Minimum.Z - currentSearchDistance - originZ) / cellWidthZ); zIndexEndEstimated = (int)((aabbOfOther.Maximum.Z + currentSearchDistance - originZ) / cellWidthZ); xIndexStart = Math.Max(xIndexStart, xIndexStartEstimated); xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax); zIndexStart = Math.Max(zIndexStart, zIndexStartEstimated); zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax); } #endregion } } } } } // Recycle temporary objects. testContactSet.Recycle(); ResourcePools.TestCollisionObjects.Recycle(testCollisionObject); extrudedTriangleGeometricObject.Recycle(); extrudedTriangleShape.Recycle(); lineSegmentGeometricObject.Recycle(); ResourcePools.LineSegmentShapes.Recycle(lineSegment); triangleGeometricObject.Recycle(); ResourcePools.TriangleShapes.Recycle(triangleShape); #endregion #region ----- Handle missing contact info ----- if (contactSet.Count == 0 && (contactSet.HaveContact && type == CollisionQueryType.Contacts || type == CollisionQueryType.ClosestPoints)) { // ----- Bad contact info: // We should have contact data because this is either a contact query and the objects touch // or this is a closest-point query. // Use our guess as the contact info. Contact closestPair = guessedClosestPair; bool isOverHole = false; if (closestPair == null) { closestPair = GuessClosestPair(contactSet, swapped, out isOverHole); } // Guesses over holes are useless. :-( if (!isOverHole) { ContactHelper.Merge(contactSet, closestPair, type, CollisionDetection.ContactPositionTolerance); } } #endregion if (CollisionDetection.FullContactSetPerFrame && type == CollisionQueryType.Contacts && numberOfContactsInLastFrame == 0 && contactSet.Count > 0 && contactSet.Count < 4) { // Try to find full contact set. // TODO: This can be optimized by not doing the whole overhead of ComputeCollision again. ContactHelper.TestWithPerturbations( CollisionDetection, contactSet, !swapped, // Perturb objectB not the height field. _computeContactsMethod); } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // From Coutinho: "Dynamic Simulations of Multibody Systems" and // Bergen: "Collision Detection in Interactive 3D Environments". // Object A should be the box. // Object B should be the sphere. IGeometricObject boxObject = contactSet.ObjectA.GeometricObject; IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject; // Swap objects if necessary. bool swapped = (sphereObject.Shape is BoxShape); if (swapped) { MathHelper.Swap(ref boxObject, ref sphereObject); } BoxShape boxShape = boxObject.Shape as BoxShape; SphereShape sphereShape = sphereObject.Shape as SphereShape; // Check if collision objects shapes are correct. if (boxShape == null || sphereShape == null) { throw new ArgumentException("The contact set must contain a box and a sphere.", "contactSet"); } Vector3F scaleBox = Vector3F.Absolute(boxObject.Scale); Vector3F scaleSphere = Vector3F.Absolute(sphereObject.Scale); // Call other algorithm for non-uniformly scaled spheres. if (scaleSphere.X != scaleSphere.Y || scaleSphere.Y != scaleSphere.Z) { if (_fallbackAlgorithm == null) { _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(BoxShape), typeof(ConvexShape)]; } _fallbackAlgorithm.ComputeCollision(contactSet, type); return; } // Apply scale. Vector3F boxExtent = boxShape.Extent * scaleBox; float sphereRadius = sphereShape.Radius * scaleSphere.X; // ----- First transform sphere center into the local space of the box. Pose boxPose = boxObject.Pose; Vector3F sphereCenterWorld = sphereObject.Pose.Position; Vector3F sphereCenter = boxPose.ToLocalPosition(sphereCenterWorld); Vector3F p = Vector3F.Zero; bool sphereCenterIsContainedInBox = true; // When sphere center is on a box surface we have to choose a suitable normal. // otherwise the normal will be computed later. Vector3F normal = Vector3F.Zero; #region ----- Look for the point p of the box that is closest to center of the sphere. ----- Vector3F boxHalfExtent = 0.5f * boxExtent; // x component if (sphereCenter.X < -boxHalfExtent.X) { p.X = -boxHalfExtent.X; sphereCenterIsContainedInBox = false; } else if (sphereCenter.X > boxHalfExtent.X) { p.X = boxHalfExtent.X; sphereCenterIsContainedInBox = false; } else { p.X = sphereCenter.X; } // y component if (sphereCenter.Y < -boxHalfExtent.Y) { p.Y = -boxHalfExtent.Y; sphereCenterIsContainedInBox = false; } else if (sphereCenter.Y > boxHalfExtent.Y) { p.Y = boxHalfExtent.Y; sphereCenterIsContainedInBox = false; } else { p.Y = sphereCenter.Y; } // z component if (sphereCenter.Z < -boxHalfExtent.Z) { p.Z = -boxHalfExtent.Z; sphereCenterIsContainedInBox = false; } else if (sphereCenter.Z > boxHalfExtent.Z) { p.Z = boxHalfExtent.Z; sphereCenterIsContainedInBox = false; } else { p.Z = sphereCenter.Z; } if (sphereCenterIsContainedInBox || (sphereCenter - p).IsNumericallyZero) { // Special case: Sphere center is within box. In this case p == center. // Lets return a point on the surface of the box. // Lets find the axis with the smallest way out (penetration depth). Vector3F diff = boxHalfExtent - Vector3F.Absolute(sphereCenter); if (diff.X <= diff.Y && diff.X <= diff.Z) { // Point on one of the x surfaces is nearest. // Check whether positive or negative surface. bool positive = (sphereCenter.X > 0); p.X = positive ? boxHalfExtent.X : -boxHalfExtent.X; if (Numeric.IsZero(diff.X)) { // Sphere center is on box surface. normal = positive ? Vector3F.UnitX : -Vector3F.UnitX; } } else if (diff.Y <= diff.X && diff.Y <= diff.Z) { // Point on one of the y surfaces is nearest. // Check whether positive or negative surface. bool positive = (sphereCenter.Y > 0); p.Y = positive ? boxHalfExtent.Y : -boxHalfExtent.Y; if (Numeric.IsZero(diff.Y)) { // Sphere center is on box surface. normal = positive ? Vector3F.UnitY : -Vector3F.UnitY; } } else { // Point on one of the z surfaces is nearest. // Check whether positive or negative surface. bool positive = (sphereCenter.Z > 0); p.Z = positive ? boxHalfExtent.Z : -boxHalfExtent.Z; if (Numeric.IsZero(diff.Z)) { // Sphere center is on box surface. normal = positive ? Vector3F.UnitZ : -Vector3F.UnitZ; } } } #endregion // ----- Convert back to world space p = boxPose.ToWorldPosition(p); Vector3F sphereCenterToP = p - sphereCenterWorld; // Compute penetration depth. float penetrationDepth = sphereCenterIsContainedInBox ? sphereRadius + sphereCenterToP.Length : sphereRadius - sphereCenterToP.Length; 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; } // ----- Create collision info. // Compute normal if we haven't set one yet. if (normal == Vector3F.Zero) { Debug.Assert(!sphereCenterToP.IsNumericallyZero, "When the center of the sphere lies on the box surface a normal should be have been set explicitly."); normal = sphereCenterIsContainedInBox ? sphereCenterToP : -sphereCenterToP; normal.Normalize(); } else { normal = boxPose.ToWorldDirection(normal); } // Position = point between sphere and box surface. Vector3F position = p - normal * (penetrationDepth / 2); if (swapped) { normal = -normal; } // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Ray vs. sphere has at max 1 contact. Debug.Assert(contactSet.Count <= 1); // Object A should be the ray. // Object B should be the sphere. IGeometricObject rayObject = contactSet.ObjectA.GeometricObject; IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject; // Swap if necessary. bool swapped = (sphereObject.Shape is RayShape); if (swapped) { MathHelper.Swap(ref rayObject, ref sphereObject); } RayShape rayShape = rayObject.Shape as RayShape; SphereShape sphereShape = sphereObject.Shape as SphereShape; // Check if shapes are correct. if (rayShape == null || sphereShape == null) { throw new ArgumentException("The contact set must contain a ray and a sphere.", "contactSet"); } // Get transformations. Vector3F rayScale = rayObject.Scale; Vector3F sphereScale = Vector3F.Absolute(sphereObject.Scale); Pose rayPose = rayObject.Pose; Pose spherePose = sphereObject.Pose; // Call other algorithm for non-uniformly scaled spheres. if (sphereScale.X != sphereScale.Y || sphereScale.Y != sphereScale.Z) { if (_fallbackAlgorithm == null) { _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(RayShape), typeof(ConvexShape)]; } _fallbackAlgorithm.ComputeCollision(contactSet, type); return; } // Scale ray and transform ray to world space. Ray rayWorld = new Ray(rayShape); rayWorld.Scale(ref rayScale); rayWorld.ToWorld(ref rayPose); // Scale sphere. float sphereRadius = sphereShape.Radius * sphereScale.X; float sphereRadiusSquared = sphereRadius * sphereRadius; // Call line segment vs. sphere for closest points queries. if (type == CollisionQueryType.ClosestPoints || type == CollisionQueryType.Boolean) { // ----- Find point on ray closest to the sphere. // Make a line segment vs. point (sphere center) check. Debug.Assert(rayWorld.Direction.IsNumericallyNormalized, "The ray direction should be normalized."); LineSegment segment = new LineSegment(rayWorld.Origin, rayWorld.Origin + rayWorld.Direction * rayWorld.Length); Vector3F segmentPoint; Vector3F sphereCenter = spherePose.Position; GeometryHelper.GetClosestPoints(segment, sphereCenter, out segmentPoint); Vector3F normal = sphereCenter - segmentPoint; float distanceSquared = normal.LengthSquared; float penetrationDepthSquared = sphereRadiusSquared - distanceSquared; contactSet.HaveContact = penetrationDepthSquared >= 0; if (type == CollisionQueryType.Boolean) { return; } if (penetrationDepthSquared <= 0) { // Separated or touching objects. Vector3F position; if (normal.TryNormalize()) { Vector3F spherePoint = sphereCenter - normal * sphereRadius; position = (spherePoint + segmentPoint) / 2; } else { position = segmentPoint; normal = Vector3F.UnitY; } if (swapped) { normal = -normal; } float penetrationDepth = -((float)Math.Sqrt(distanceSquared) - sphereRadius); // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, contactSet.HaveContact); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); return; } // Otherwise, we have a penetrating contact. // Compute 1 contact ... } // First assume no contact. contactSet.HaveContact = false; // See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", pp. 70. // Compute in sphere local space. // Transform ray to local space of sphere. Ray ray = rayWorld; ray.ToLocal(ref spherePose); Vector3F s = ray.Origin; // ray source Vector3F d = ray.Direction; // ray direction Vector3F r = d * ray.Length; // ray float rayLengthSquared = ray.Length * ray.Length; // ||r||² float δ = -Vector3F.Dot(s, r); // -s∙r float σ = δ * δ - rayLengthSquared * (s.LengthSquared - sphereRadiusSquared); if (σ >= 0) { // The infinite ray intersects. float sqrtσ = (float)Math.Sqrt(σ); float λ2 = (δ + sqrtσ) /* / rayLengthSquared */; // Division can be ignored. Only sign is relevant. if (λ2 >= 0) { // Ray shoots to sphere. float λ1 = (δ - sqrtσ) / rayLengthSquared; if (λ1 <= 1) { // Ray hits sphere. contactSet.HaveContact = true; Debug.Assert(type != CollisionQueryType.Boolean); // Was handled before. float penetrationDepth; Vector3F normal; if (λ1 > 0) { // λ1 shows the entry point. λ2 shows the exit point. penetrationDepth = λ1 * ray.Length; // Distance from ray origin to entry point (hit). normal = -(s + r * λ1); // Entry point (hit). } else { // Ray origin is in the sphere. penetrationDepth = 0; normal = -s; } Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth; normal = spherePose.ToWorldDirection(normal); if (!normal.TryNormalize()) { normal = Vector3F.UnitY; } if (swapped) { normal = -normal; } // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } } } }
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; Vector3F scaleA = geometricObjectA.Scale; Vector3F 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>)) { #region ----- TriangleMesh with BVH vs. TriangleMesh with BVH ----- 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); } #endregion } 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 = Vector3F.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>)) { #region ----- TriangleMesh BVH vs. * ----- // 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(Vector3F.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); } #endregion } else { #region ----- TriangleMesh vs. * ----- // 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. Vector3F innerPointA = poseA.ToWorldPosition(geometricObjectA.Shape.InnerPoint * scaleA); Vector3F 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. Vector3F minimum = aabbOfB.Minimum - new Vector3F(closestPairDistance); Vector3F maximum = aabbOfB.Maximum + new Vector3F(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 Vector3F(closestPairDistance); searchSpaceAabb.Maximum = aabbOfB.Maximum + new Vector3F(closestPairDistance); } } } #endregion } } } finally { testContactSet.Recycle(); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB); testGeometricObjectB.Recycle(); testGeometricObjectA.Recycle(); ResourcePools.TriangleShapes.Recycle(testTriangleShapeB); ResourcePools.TriangleShapes.Recycle(testTriangleShapeA); } }
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; Vector3F 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 = Vector3F.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 Vector3F triangleNormalLocal = Vector3F.Cross(triangle.Vertex1 - triangle.Vertex0, triangle.Vertex2 - triangle.Vertex0) / scaleA; Vector3F 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 = Vector3F.Dot(testContactSet[0].Normal, preferredNormal); if (contactDotTriangle < WeldingLimit) { // Bad normal. Perform welding. Vector3F contactPositionOnTriangle = swapped ? testContactSet[0].PositionBLocal / scaleA : testContactSet[0].PositionALocal / scaleA; Vector3F 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 = Vector3F.Zero; if (testContactSet.Count > 0) { Contact c1 = testContactSet[0]; float contact1DotTriangle = Vector3F.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(); } } } } } #region ----- Merge testContactSet into contactSet ----- 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); } #endregion } }
// 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); } }