private bool HaveContact(IGeometricObject clipGeometry, Vector3F position) { // Use a shared collision detection instance. var collisionDetection = SceneHelper.CollisionDetection; if (_sphereCollisionObject == null) { // First time initializations. var sphereGeometricObject = TestGeometricObject.Create(); sphereGeometricObject.Shape = new SphereShape(0); _sphereCollisionObject = new CollisionObject(sphereGeometricObject); _clipCollisionObject = new CollisionObject(TestGeometricObject.Create()); } var _sphereGeometricObject = (TestGeometricObject)_sphereCollisionObject.GeometricObject; _sphereGeometricObject.Pose = new Pose(position); _clipCollisionObject.GeometricObject = clipGeometry; var result = collisionDetection.HaveContact(_sphereCollisionObject, _clipCollisionObject); _clipCollisionObject.GeometricObject = null; return(result); }
public void Events() { // The events are "empty". This is only for full code coverage: var g = TestGeometricObject.Create(); g.PoseChanged += OnChanged; g.ShapeChanged += OnChanged; g.Pose = new Pose(new Vector3(1, 2, 3)); g.Shape = new BoxShape(1, 2, 3); g.PoseChanged -= OnChanged; g.ShapeChanged -= OnChanged; }
/// <inheritdoc/> /// <exception cref="ArgumentException"> /// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> contains a /// <see cref="HeightField"/>. /// </exception> /// <exception cref="ArgumentNullException"> /// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>. /// </exception> /// <exception cref="NotSupportedException"> /// <paramref name="objectA"/> or <paramref name="objectB"/> contains a /// <see cref="HeightField"/> with a negative scaling. Computing collisions for height fields /// with a negative scaling is not supported. /// </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 height field, swap objects if necessary. if (!(objectA.GeometricObject.Shape is HeightField)) { MathHelper.Swap(ref objectA, ref objectB); MathHelper.Swap(ref targetPoseA, ref targetPoseB); } IGeometricObject geometricObjectA = objectA.GeometricObject; IGeometricObject geometricObjectB = objectB.GeometricObject; HeightField heightFieldA = geometricObjectA.Shape as HeightField; // Check if collision object shapes are correct. if (heightFieldA == null) { throw new ArgumentException("One object must be a height field."); } // Height field vs height field makes no sense. if (objectB.GeometricObject.Shape is HeightField) { return(1); } Pose startPoseA = geometricObjectA.Pose; Pose startPoseB = geometricObjectB.Pose; Vector3 scaleA = geometricObjectA.Scale; Vector3 scaleB = geometricObjectB.Scale; // We do not support negative scaling (see comments in ComputeCollision). if (scaleA.X < 0 || scaleA.Y < 0 || scaleA.Z < 0) { throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported."); } // 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()]; // Get height field and basic info. int arrayLengthX = heightFieldA.NumberOfSamplesX; int arrayLengthZ = heightFieldA.NumberOfSamplesZ; Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell)."); float cellWidthX = heightFieldA.WidthX * scaleA.X / (arrayLengthX - 1); float cellWidthZ = heightFieldA.WidthZ * scaleA.Z / (arrayLengthZ - 1); float originX = heightFieldA.OriginX; float originZ = heightFieldA.OriginZ; // ----- Compute the cell indices for the AABB. // Estimate start and end indices from our search distance. int xIndexStartEstimated = (int)((aabbSweptBInA.Minimum.X - originX) / cellWidthX); int xIndexEndEstimated = (int)((aabbSweptBInA.Maximum.X - originX) / cellWidthX); int zIndexStartEstimated = (int)((aabbSweptBInA.Minimum.Z - originZ) / cellWidthZ); int zIndexEndEstimated = (int)((aabbSweptBInA.Maximum.Z - 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); float timeOfImpact = 1; 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 = heightFieldA.GetTriangle(xIndex, zIndex, triangleIndex != 0); // 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); } }
public void Recycle() { ObjectA = null; ObjectB = null; Pool.Recycle(this); }
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); }
// 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) { // Object B should be the transformed shape. CollisionObject objectA = contactSet.ObjectA; CollisionObject objectB = contactSet.ObjectB; // Swap objects if necessary. bool swapped = !(objectB.GeometricObject.Shape is TransformedShape); if (swapped) { MathHelper.Swap(ref objectA, ref objectB); } IGeometricObject geometricObjectB = objectB.GeometricObject; TransformedShape transformedShape = geometricObjectB.Shape as TransformedShape; // Check if collision objects shapes are correct. if (transformedShape == null) { throw new ArgumentException("The contact set must contain a transformed shape.", "contactSet"); } // Assume no contact. contactSet.HaveContact = false; // Currently object B has the following structure: // // CollisionObject objectB // GeometricObject geometricObjectB // Pose poseB // Scale scaleB // TransformedShape transformedShape // GeometricObject childGeometricObject // Pose childPose // Scale childScale // Shape childShape // // To compute the collisions we temporarily remove the TransformedShape: // We replace the original geometric object with a test geometric object and combine the // transformations: // // CollisionObject testObjectB // GeometricObject testGeometricObjectB // Pose poseB * childPose // Shape childShape // Scale scaleB * childScale Pose poseB = geometricObjectB.Pose; Vector3F scaleB = geometricObjectB.Scale; // Apply scale to pose and test geometric object. // (Note: The scaling is either uniform or the transformed object 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.) IGeometricObject childGeometricObject = transformedShape.Child; Pose childPose = childGeometricObject.Pose; // Non-uniform scaling is not supported for rotated child objects. if ((scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z) && childPose.HasRotation) { throw new NotSupportedException("Computing collisions for transformed shapes with local rotations and non-uniform scaling is not supported."); } childPose.Position *= scaleB; // Apply scaling to local translation. var testGeometricObjectB = TestGeometricObject.Create(); testGeometricObjectB.Shape = childGeometricObject.Shape; testGeometricObjectB.Scale = scaleB * childGeometricObject.Scale; // Apply scaling to local scale. testGeometricObjectB.Pose = poseB * childPose; var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain(); testCollisionObjectB.SetInternal(objectB, testGeometricObjectB); var testContactSet = swapped ? ContactSet.Create(testCollisionObjectB, objectA) : ContactSet.Create(objectA, testCollisionObjectB); testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed; // Transform contacts into space of child and copy them into the testContactSet. //int numberOfOldContacts = contactSet.Count; //for (int i = 0; i < numberOfOldContacts; i++) //{ // Contact contact = contactSet[i]; // if (swapped) // contact.PositionALocal = childPose.ToLocalPosition(contact.PositionALocal); // else // contact.PositionBLocal = childPose.ToLocalPosition(contact.PositionBLocal); // // testContactSet.Add(contact); //} // Compute collision. var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[objectA, testCollisionObjectB]; collisionAlgorithm.ComputeCollision(testContactSet, type); if (testContactSet.HaveContact) { contactSet.HaveContact = true; } // Transform contacts into space of parent TransformShape. int numberOfNewContacts = testContactSet.Count; for (int i = 0; i < numberOfNewContacts; i++) { Contact contact = testContactSet[i]; if (swapped) { contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal); } else { contact.PositionBLocal = childPose.ToWorldPosition(contact.PositionBLocal); } } // Merge new contacts to contactSet. // (If testContactSet contains all original contacts (see commented out part above), we can // simply clear contactSet and copy all contacts of testContactSet.) //contactSet.Clear(); //foreach (Contact contact in testContactSet) // contactSet.Add(contact); ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance); // Recycle temporary objects. testContactSet.Recycle(); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB); testGeometricObjectB.Recycle(); }
// See FAST paper: "Interactive Continuous Collision Detection for Non-Convex Polyhedra", Kim Young et al. /// <summary> /// Gets the time of impact using Conservative Advancement. /// </summary> /// <param name="objectA">The object A.</param> /// <param name="targetPoseA">The target pose of A.</param> /// <param name="objectB">The object B.</param> /// <param name="targetPoseB">The target pose of B.</param> /// <param name="allowedPenetrationDepth">The allowed penetration depth.</param> /// <param name="collisionDetection">The collision detection.</param> /// <returns> /// The time of impact in the range [0, 1]. /// </returns> /// <remarks> /// This algorithm does not work for concave objects. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="objectA"/>, <paramref name="objectB"/> or /// <paramref name="collisionDetection"/> is <see langword="null"/>. /// </exception> internal static float GetTimeOfImpactCA(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetrationDepth, CollisionDetection collisionDetection) // Required for collision algorithm matrix. { if (objectA == null) { throw new ArgumentNullException("objectA"); } if (objectB == null) { throw new ArgumentNullException("objectB"); } if (collisionDetection == null) { throw new ArgumentNullException("collisionDetection"); } IGeometricObject geometricObjectA = objectA.GeometricObject; IGeometricObject geometricObjectB = objectB.GeometricObject; Pose startPoseA = geometricObjectA.Pose; Pose startPoseB = geometricObjectB.Pose; // Get angular velocity ω of object A (as magnitude + rotation axis). // qEnd = ∆q * qStart // => ∆q = qEnd * qStart.Inverse QuaternionF qA = QuaternionF.CreateRotation(targetPoseA.Orientation * startPoseA.Orientation.Transposed); // ω = ∆α / ∆t, ∆t = 1 // => ω = ∆α float ωA = qA.Angle; // Magnitude |ω| Vector3F ωAxisA = (!Numeric.AreEqual(qA.W, 1)) ? qA.Axis : Vector3F.UnitX; // Rotation axis of ω // Get angular velocity ω of object B (as magnitude + rotation axis). // (Same as above.) QuaternionF qB = QuaternionF.CreateRotation(targetPoseB.Orientation * startPoseB.Orientation.Transposed); float ωB = qB.Angle; // Magnitude |ω| Vector3F ωAxisB = (!Numeric.AreEqual(qB.W, 1)) ? qB.Axis : Vector3F.UnitX; // Rotation axis of ω // Bounding sphere radii. float rMaxA = GetBoundingRadius(geometricObjectA); float rMaxB = GetBoundingRadius(geometricObjectB); // |ω| * rMax is the angular part of the projected velocity bound. float angularVelocityProjected = ωA * rMaxA + ωB * rMaxB; // Compute relative linear velocity. // (linearRelVel ∙ normal > 0 if objects are getting closer.) Vector3F linearVelocityA = targetPoseA.Position - startPoseA.Position; Vector3F linearVelocityB = targetPoseB.Position - startPoseB.Position; Vector3F linearVelocityRelative = linearVelocityA - linearVelocityB; // Abort if relative movement is zero. if (Numeric.IsZero(linearVelocityRelative.Length + angularVelocityProjected)) { return(1); } var distanceAlgorithm = collisionDetection.AlgorithmMatrix[objectA, objectB]; // Use temporary test objects. var testGeometricObjectA = TestGeometricObject.Create(); testGeometricObjectA.Shape = geometricObjectA.Shape; testGeometricObjectA.Scale = geometricObjectA.Scale; testGeometricObjectA.Pose = startPoseA; var testGeometricObjectB = TestGeometricObject.Create(); testGeometricObjectB.Shape = geometricObjectB.Shape; testGeometricObjectB.Scale = geometricObjectB.Scale; testGeometricObjectB.Pose = startPoseB; var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain(); testCollisionObjectA.SetInternal(objectA, testGeometricObjectA); var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain(); testCollisionObjectB.SetInternal(objectB, testGeometricObjectB); var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB); try { distanceAlgorithm.UpdateClosestPoints(testContactSet, 0); if (testContactSet.Count < 0) { // No distance result --> Abort. return(1); } Vector3F normal = testContactSet[0].Normal; float distance = -testContactSet[0].PenetrationDepth; float λ = 0; float λPrevious = 0; for (int i = 0; i < MaxNumberOfIterations && distance > 0; i++) { // |v∙n| float linearVelocityProject = Vector3F.Dot(linearVelocityRelative, normal); // |n x ω| * rMax angularVelocityProjected = Vector3F.Cross(normal, ωAxisA).Length *ωA *rMaxA + Vector3F.Cross(normal, ωAxisB).Length *ωB *rMaxB; // Total projected velocity. float velocityProjected = linearVelocityProject + angularVelocityProjected; // Abort for separating objects. if (Numeric.IsLess(velocityProjected, 0)) { break; } // Increase TOI. float μ = (distance + allowedPenetrationDepth) / velocityProjected; λ = λ + μ; if (λ < 0 || λ > 1) { break; } Debug.Assert(λPrevious < λ); if (λ <= λPrevious) { break; } // Get new interpolated poses. Vector3F positionA = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position); Matrix33F rotationA = Matrix33F.CreateRotation(ωAxisA, λ * ωA); testGeometricObjectA.Pose = new Pose(positionA, rotationA * startPoseA.Orientation); Vector3F positionB = startPoseB.Position + λ * (targetPoseB.Position - startPoseB.Position); Matrix33F rotationB = Matrix33F.CreateRotation(ωAxisB, λ * ωB); testGeometricObjectB.Pose = new Pose(positionB, rotationB * startPoseB.Orientation); // Get new closest point distance. distanceAlgorithm.UpdateClosestPoints(testContactSet, 0); if (testContactSet.Count == 0) { break; } normal = testContactSet[0].Normal; distance = -testContactSet[0].PenetrationDepth; λPrevious = λ; } if (testContactSet.HaveContact && λ > 0 && λ < 1 && testContactSet.Count > 0) { return(λ); // We already have a contact that we could use. // result.Contact = testContactSet[0]; } } finally { // Recycle temporary objects. testContactSet.Recycle(true); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB); testGeometricObjectA.Recycle(); testGeometricObjectB.Recycle(); } return(1); }
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); } }
/// <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; Vector3F scaleA = geometricObjectA.Scale; Vector3F 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 = Vector3F.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(Vector3F.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); }
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 } }
/// <summary> /// Determines whether the specified world space position is underwater. /// </summary> /// <param name="position">The position in world space.</param> /// <returns> /// <see langword="true"/> if the position is underwater; otherwise, <see langword="false"/> /// </returns> /// <remarks> /// A position is underwater if it is inside the <see cref="Shape"/> of this node. /// </remarks> public bool IsUnderwater(Vector3F position) { //if (!EnableUnderwaterEffect) // return false; // Oceans are treated like a horizontal plane through the node origin. if (Volume == null) { return(position.Y < PoseWorld.Position.Y); } // Thread-safety: We lock this operation because all tests use the same cache // and test objects. lock (_underwaterTestLock) { // Return cached result if the point and the water pose/shape are still the same. if (!IsDirty) { if (Vector3F.AreNumericallyEqual(position, _lastTestPosition)) { return(_lastTestResult); } } else { // Clear flag. We will cache a new result. IsDirty = false; } _lastTestPosition = position; _lastTestResult = false; // Use a shared collision detection instance. var collisionDetection = SceneHelper.CollisionDetection; if (_sphereShape == null) { // First time initializations. _sphereShape = new SphereShape(0); _rayShape = new RayShape(); _testCollisionObject = new CollisionObject(TestGeometricObject.Create()); _waterCollisionObject = new CollisionObject(TestGeometricObject.Create()); } var testGeometricObject = (TestGeometricObject)_testCollisionObject.GeometricObject; var waterGeometricObject = (TestGeometricObject)_waterCollisionObject.GeometricObject; try { // Initialize water collision object. waterGeometricObject.Shape = Volume; waterGeometricObject.Scale = ScaleWorld; waterGeometricObject.Pose = PoseWorld; // Test if point touches underwater volume. (Skip this test for triangle mesh shapes.) if (!(Shape is TriangleMeshShape)) { testGeometricObject.Pose = new Pose(position); testGeometricObject.Shape = _sphereShape; if (collisionDetection.HaveContact(_testCollisionObject, _waterCollisionObject)) { _lastTestResult = true; return(true); } // For convex shapes, the above test is sufficient. if (Shape is ConvexShape) { return(false); } } // For triangle meshes - which are hollow - we have to make a more complex test. // We shoot vertical rays and check if we hit the underwater volume surface. // Make explicit point vs. AABB test first. if (!collisionDetection.HaveAabbContact(_testCollisionObject, _waterCollisionObject)) { return(false); } // Switch to ray shape. testGeometricObject.Shape = _rayShape; // Shoot down. Start 1 unit above the surface. Vector3F origin = position; origin.Y = Math.Max(Aabb.Maximum.Y, origin.Y) + 1; _rayShape.Origin = origin; _rayShape.Length = (origin - position).Length; _rayShape.Direction = Vector3F.Down; if (!collisionDetection.HaveContact(_testCollisionObject, _waterCollisionObject)) { return(false); // Camera is above water. } // Shoot up. Start 1 m under the water volume. origin = position; origin.Y = Math.Min(Aabb.Minimum.Y, origin.Y) - 1; _rayShape.Origin = origin; _rayShape.Length = (origin - position).Length; _rayShape.Direction = Vector3F.Up; _lastTestResult = collisionDetection.HaveContact(_testCollisionObject, _waterCollisionObject); return(_lastTestResult); } finally { // Remove references to avoid "memory leaks". waterGeometricObject.Shape = Volume; } } }
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; } } Vector3 scaleHeightField = heightFieldGeometricObject.Scale; Vector3 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. Vector3 heightFieldUpAxis = heightFieldPose.ToWorldDirection(Vector3.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; // 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 = Vector3.Zero; lineSegment.End = -heightField.Depth * Vector3.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. Vector3 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 (Vector3.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; // Get barycentric coordinates of contact position. Vector3 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 Vector3(float.NaN); } } else { if (xIndex > 0) { neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex - 1, zIndex, true).Normal / scaleHeightField); neighborNormal.TryNormalize(); } else { neighborNormal = new Vector3(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 Vector3(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 Vector3(float.NaN); } } } } // Contact normals in the range triangleNormal - neighborNormal are allowed. // Others, especially vertical contacts in slopes or horizontal normals are not // allowed. var cosMinAngle = Vector3.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 = Vector3.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) && Vector3.Dot(heightFieldUpAxis, triangleNormal) < WeldingLimit) { testContactSet.PreferredNormal = (swapped) ? -triangleNormal : triangleNormal; collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts); testContactSet.PreferredNormal = Vector3.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); // 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); } } } } } } // 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); 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); } } 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); } }
// 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; Vector3 scaleA = geometricObjectA.Scale; Vector3 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="TransformedShape"/>. /// <exception cref="ArgumentNullException"> /// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>. /// </exception> /// </exception> public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration) { // Most of this code is copied from ComputeCollision() above. // We get the child and compute the TOI for the child movement. 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. if (objectA == null) { throw new ArgumentNullException("objectA"); } if (objectB == null) { throw new ArgumentNullException("objectB"); } // B should be the transformed shape, swap objects if necessary. bool swapped = !(objectB.GeometricObject.Shape is TransformedShape); if (swapped) { MathHelper.Swap(ref objectA, ref objectB); MathHelper.Swap(ref targetPoseA, ref targetPoseB); } IGeometricObject geometricObjectB = objectB.GeometricObject; TransformedShape transformedShape = geometricObjectB.Shape as TransformedShape; // Check if collision objects shapes are correct. if (transformedShape == null) { throw new ArgumentException("objectA or objectB must be a TransformedShape."); } 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. // Apply scale to pose and test geometric object. // (Note: The scaling is either uniform or the transformed object 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.) IGeometricObject childGeometricObject = transformedShape.Child; Pose childPose = childGeometricObject.Pose; childPose.Position *= scaleB; // Apply scaling to local translation. var testGeometricObjectB = TestGeometricObject.Create(); testGeometricObjectB.Shape = childGeometricObject.Shape; testGeometricObjectB.Scale = scaleB * childGeometricObject.Scale; // Apply scaling to local scale. testGeometricObjectB.Pose = poseB * childPose; var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain(); testCollisionObjectB.SetInternal(objectB, testGeometricObjectB); // Compute TOI. var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[objectA, testCollisionObjectB]; float timeOfImpact = collisionAlgorithm.GetTimeOfImpact( objectA, targetPoseA, testCollisionObjectB, targetPoseB * childPose, allowedPenetration); // Recycle temporary objects. ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB); testGeometricObjectB.Recycle(); return(timeOfImpact); }
/// <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; Vector3 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); }
/// <summary> /// Performs more collision tests while slightly rotating one collision object. /// </summary> /// <param name="collisionDetection">The collision detection.</param> /// <param name="contactSet"> /// The contact set; must contain at least 1 <see cref="Contact"/>. /// </param> /// <param name="perturbB"> /// if set to <see langword="true"/> collision object B will be rotated; otherwise collision /// object A will be rotated. /// </param> /// <param name="testMethod">The test method that is called to compute contacts.</param> /// <remarks> /// This method rotates one object 3 times and calls contact computation for the new /// orientations. It is recommended to call this method only when the contact set has 1 new /// contact. /// </remarks> internal static void TestWithPerturbations(CollisionDetection collisionDetection, ContactSet contactSet, bool perturbB, Action <ContactSet> testMethod) { Debug.Assert(contactSet != null); Debug.Assert(contactSet.Count > 0 && contactSet.HaveContact || !contactSet.IsPerturbationTestAllowed); Debug.Assert(testMethod != null); // Make this test only if there is 1 contact. // If there are 0 contacts, we assume that the contact pair is separated. // If there are more than 3 contacts, then we already have a lot of contacts to work with, no // need to search for more. if (!contactSet.HaveContact || contactSet.Count == 0 || contactSet.Count >= 4 || !contactSet.IsPerturbationTestAllowed) { return; } // Get data of object that will be rotated. var collisionObject = (perturbB) ? contactSet.ObjectB : contactSet.ObjectA; var geometricObject = collisionObject.GeometricObject; var pose = geometricObject.Pose; // Get normal, pointing to the test object. var normal = contactSet[0].Normal; if (!perturbB) { normal = -normal; } var contactPosition = contactSet[0].Position; var centerToContact = contactPosition - pose.Position; // Compute a perturbation angle proportional to the dimension of the object. var radius = geometricObject.Aabb.Extent.Length; var angle = collisionDetection.ContactPositionTolerance / radius; // axis1 is in the contact tangent plane, orthogonal to normal. var axis1 = Vector3.Cross(normal, centerToContact); // If axis1 is zero then normal and centerToContact are collinear. This happens // for example for spheres or cone tips against flat faces. In these cases we assume // that there will be max. 1 contact. if (axis1.IsNumericallyZero) { return; } var axis1Local = pose.ToLocalDirection(axis1); var rotation = Matrix.CreateRotation(axis1Local, -angle); // Use temporary test objects. var testGeometricObject = TestGeometricObject.Create(); testGeometricObject.Shape = geometricObject.Shape; testGeometricObject.Scale = geometricObject.Scale; testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation); var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain(); testCollisionObject.SetInternal(collisionObject, testGeometricObject); var testContactSet = perturbB ? ContactSet.Create(contactSet.ObjectA, testCollisionObject) : ContactSet.Create(testCollisionObject, contactSet.ObjectB); testContactSet.IsPerturbationTestAllowed = false; // Avoid recursive perturbation tests! testContactSet.PreferredNormal = contactSet.PreferredNormal; // Compute next contacts. testMethod(testContactSet); if (testContactSet.Count > 0) { // axis2 is in the contact tangent plane, orthogonal to axis1. var axis2 = Vector3.Cross(axis1, normal); var axis2Local = pose.ToLocalDirection(axis2); var rotation2 = Matrix.CreateRotation(axis2Local, -angle); testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2); // Compute next contacts. testMethod(testContactSet); // Invert rotation2. rotation2 = Matrix.Transpose(rotation2); testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2); // Compute next contacts. testMethod(testContactSet); } // Set HaveContact. It is reset when a perturbation separates the objects. testContactSet.HaveContact = true; // TODO: Test if we need this: // The contact world positions are not really correct because one object was rotated. // UpdateContacts recomputes the world positions from the local positions. UpdateContacts(testContactSet, 0, collisionDetection.ContactPositionTolerance); // Merge contacts of testContactSet into contact set, but do not change existing contacts. foreach (var contact in testContactSet) { // We call TryMerge() to see if the contact is similar to an existing contact. bool exists = TryMergeWithNearestContact( contactSet, contact, collisionDetection.ContactPositionTolerance, false); // The existing contact must no be changed! if (exists) { // We can throw away the new contact because a similar is already in the contact set. contact.Recycle(); } else { // Add new contact. contactSet.Add(contact); } } // Recycle temporary objects. testContactSet.Recycle(); ResourcePools.TestCollisionObjects.Recycle(testCollisionObject); testGeometricObject.Recycle(); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { CollisionObject collisionObjectA = contactSet.ObjectA; CollisionObject collisionObjectB = contactSet.ObjectB; IGeometricObject geometricObjectA = collisionObjectA.GeometricObject; IGeometricObject geometricObjectB = collisionObjectB.GeometricObject; // Object A should be the composite, swap objects if necessary. // When testing CompositeShape vs. CompositeShape with BVH, object A should be the // CompositeShape with BVH. CompositeShape compositeShapeA = geometricObjectA.Shape as CompositeShape; CompositeShape compositeShapeB = geometricObjectB.Shape as CompositeShape; bool swapped = false; if (compositeShapeA == null) { // Object A is something else. Object B must be a composite shape. swapped = true; } else if (compositeShapeA.Partition == null && compositeShapeB != null && compositeShapeB.Partition != null) { // Object A has no BVH, object B is CompositeShape with BVH. swapped = true; } if (swapped) { MathHelper.Swap(ref collisionObjectA, ref collisionObjectB); MathHelper.Swap(ref geometricObjectA, ref geometricObjectB); MathHelper.Swap(ref compositeShapeA, ref compositeShapeB); } // Check if collision objects shapes are correct. if (compositeShapeA == null) { throw new ArgumentException("The contact set must contain a composite shape.", "contactSet"); } // Assume no contact. contactSet.HaveContact = false; Vector3 scaleA = geometricObjectA.Scale; Vector3 scaleB = geometricObjectB.Scale; // Check if transforms are supported. if (compositeShapeA != null && // When object A is a CompositeShape (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z) && // non-uniform scaling is not supported compositeShapeA.Children.Any(child => child.Pose.HasRotation)) // when a child has a local rotation. { // Note: Any() creates garbage, but non-uniform scalings should not be used anyway. throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported."); } // Same check for object B. if (compositeShapeB != null && (scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z) && compositeShapeB.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway. { throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported."); } // ----- A few fixed objects which are reused to avoid GC garbage. var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain(); var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain(); // Create a test contact set and initialize with dummy objects. // (The actual collision objects are set below.) var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB); var testGeometricObjectA = TestGeometricObject.Create(); var testGeometricObjectB = TestGeometricObject.Create(); try { if (compositeShapeA.Partition != null && (type != CollisionQueryType.ClosestPoints || compositeShapeA.Partition is ISupportClosestPointQueries <int>)) { if (compositeShapeB != null && compositeShapeB.Partition != null) { Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine."); if (type != CollisionQueryType.ClosestPoints) { // ----- Boolean or Contact Query // Heuristic: Test large BVH vs. small BVH. Aabb aabbOfA = geometricObjectA.Aabb; Aabb aabbOfB = geometricObjectB.Aabb; float largestExtentA = aabbOfA.Extent.LargestComponent; float largestExtentB = aabbOfB.Extent.LargestComponent; IEnumerable <Pair <int> > overlaps; bool overlapsSwapped = largestExtentA < largestExtentB; if (overlapsSwapped) { overlaps = compositeShapeB.Partition.GetOverlaps( scaleB, geometricObjectB.Pose, compositeShapeA.Partition, scaleA, geometricObjectA.Pose); } else { overlaps = compositeShapeA.Partition.GetOverlaps( scaleA, geometricObjectA.Pose, compositeShapeB.Partition, scaleB, geometricObjectB.Pose); } foreach (var overlap in overlaps) { if (type == CollisionQueryType.Boolean && contactSet.HaveContact) { break; // We can abort early. } AddChildChildContacts( contactSet, overlapsSwapped ? overlap.Second : overlap.First, overlapsSwapped ? overlap.First : overlap.Second, type, testContactSet, testCollisionObjectA, testGeometricObjectA, testCollisionObjectB, testGeometricObjectB); } } else { // Closest-Point Query var callback = ClosestPointsCallbacks.Obtain(); callback.CollisionAlgorithm = this; callback.Swapped = false; callback.ContactSet = contactSet; callback.TestCollisionObjectA = testCollisionObjectA; callback.TestCollisionObjectB = testCollisionObjectB; callback.TestGeometricObjectA = testGeometricObjectA; callback.TestGeometricObjectB = testGeometricObjectB; callback.TestContactSet = testContactSet; ((ISupportClosestPointQueries <int>)compositeShapeA.Partition) .GetClosestPointCandidates( scaleA, geometricObjectA.Pose, compositeShapeB.Partition, scaleB, geometricObjectB.Pose, callback.HandlePair); ClosestPointsCallbacks.Recycle(callback); } } else { // Compute AABB of B in local space of the CompositeShape. Aabb aabbBInA = geometricObjectB.Shape.GetAabb( scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose); // Apply inverse scaling to do the AABB checks in the unscaled local space of A. aabbBInA.Scale(Vector3.One / scaleA); if (type != CollisionQueryType.ClosestPoints) { // Boolean or Contact Query foreach (var childIndex in compositeShapeA.Partition.GetOverlaps(aabbBInA)) { if (type == CollisionQueryType.Boolean && contactSet.HaveContact) { break; // We can abort early. } AddChildContacts( contactSet, swapped, childIndex, type, testContactSet, testCollisionObjectA, testGeometricObjectA); } } else if (type == CollisionQueryType.ClosestPoints) { // Closest-Point Query var callback = ClosestPointsCallbacks.Obtain(); callback.CollisionAlgorithm = this; callback.Swapped = swapped; callback.ContactSet = contactSet; callback.TestCollisionObjectA = testCollisionObjectA; callback.TestCollisionObjectB = testCollisionObjectB; callback.TestGeometricObjectA = testGeometricObjectA; callback.TestGeometricObjectB = testGeometricObjectB; callback.TestContactSet = testContactSet; ((ISupportClosestPointQueries <int>)compositeShapeA.Partition) .GetClosestPointCandidates( aabbBInA, float.PositiveInfinity, callback.HandleItem); ClosestPointsCallbacks.Recycle(callback); } } } else { // Compute AABB of B in local space of the composite. Aabb aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose); // Apply inverse scaling to do the AABB checks in the unscaled local space of A. aabbBInA.Scale(Vector3.One / scaleA); // Go through list of children and find contacts. int numberOfChildGeometries = compositeShapeA.Children.Count; for (int i = 0; i < numberOfChildGeometries; i++) { IGeometricObject child = compositeShapeA.Children[i]; // NOTE: For closest-point queries we could be faster estimating a search space. // See TriangleMeshAlgorithm or BVH queries. // But the current implementation is sufficient. If the CompositeShape is more complex // the user should be using spatial partitions anyway. // For boolean or contact queries, we make an AABB test first. // For closest points where we have not found a contact yet, we have to search // all children. if ((type == CollisionQueryType.ClosestPoints && !contactSet.HaveContact) || GeometryHelper.HaveContact(aabbBInA, child.Shape.GetAabb(child.Scale, child.Pose))) { // TODO: We could compute the minDistance of the child AABB and the AABB of the // other shape. If the minDistance is greater than the current closestPairDistance // we can ignore this pair. - This could be a performance boost. // Get contacts/closest pairs of this child. AddChildContacts( contactSet, swapped, i, type, testContactSet, testCollisionObjectA, testGeometricObjectA); // We have contact and stop for boolean queries. if (contactSet.HaveContact && type == CollisionQueryType.Boolean) { break; } } } } } finally { Debug.Assert(collisionObjectA.GeometricObject.Shape == compositeShapeA, "Shape was altered and not restored."); testContactSet.Recycle(); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB); testGeometricObjectB.Recycle(); testGeometricObjectA.Recycle(); } }
// 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) { 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. Vector3F rayScale = rayObject.Scale; Pose rayPose = rayObject.Pose; Vector3F 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 = Vector3F.One / compositeScale; ray.Scale(ref inverseCompositeScale); try { if (compositeShape.Partition != null) { #region ----- Composite with BVH vs. * ----- 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); } #endregion } else { #region ----- Composite vs. *----- var rayDirectionInverse = new Vector3F( 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; } } } #endregion } } finally { Debug.Assert(compositeObject.Shape == compositeShape, "Shape was altered and not restored."); testContactSet.Recycle(); ResourcePools.TestCollisionObjects.Recycle(testCollisionObject); testGeometricObject.Recycle(); } }
// 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 also used in RayCompositeAlgorithm.cs. Keep changes in sync! // Object A should be the CompositeShape. 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; // TODO: We could add existing contacts with the same child shape to childContactSet. // Collision algorithms could take advantage of existing contact information to speed up // calculations. However, at the moment the collision algorithms ignore existing contacts. // If we add the exiting contacts to childContactSet we need to uncomment the comment // code lines below. // Transform contacts into space of child shape. //foreach (Contact c in childContactSet) //{ // if (childContactSet.ObjectA == childCollisionObject) // c.PositionALocal = childPose.ToLocalPosition(c.PositionALocal); // else // c.PositionBLocal = childPose.ToLocalPosition(c.PositionBLocal); //} // 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); //if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary. //{ contact.FeatureB = childIndex; //} } else { contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal); //if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary. //{ contact.FeatureA = childIndex; //} } } // Merge child contacts. ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance); } }
// See FAST paper: "Interactive Continuous Collision Detection for Non-Convex Polyhedra", Kim Young et al. /// <summary> /// Gets the time of impact using Conservative Advancement (ignoring rotational movement). /// </summary> /// <param name="objectA">The object A.</param> /// <param name="targetPoseA">The target pose of A.</param> /// <param name="objectB">The object B.</param> /// <param name="targetPoseB">The target pose of B.</param> /// <param name="allowedPenetration">The allowed penetration depth.</param> /// <param name="collisionDetection">The collision detection.</param> /// <returns> /// The time of impact in the range [0, 1]. /// </returns> /// <remarks> /// This algorithm does not work for concave objects. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="objectA"/>, <paramref name="objectB"/> or /// <paramref name="collisionDetection"/> is <see langword="null"/>. /// </exception> internal static float GetTimeOfImpactLinearCA(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration, CollisionDetection collisionDetection) // Required for collision algorithm matrix. { if (objectA == null) { throw new ArgumentNullException("objectA"); } if (objectB == null) { throw new ArgumentNullException("objectB"); } if (collisionDetection == null) { throw new ArgumentNullException("collisionDetection"); } IGeometricObject geometricObjectA = objectA.GeometricObject; IGeometricObject geometricObjectB = objectB.GeometricObject; Pose startPoseA = geometricObjectA.Pose; Pose startPoseB = geometricObjectB.Pose; // Compute relative linear velocity. // (linearRelVel ∙ normal > 0 if objects are getting closer.) Vector3F linearVelocityA = targetPoseA.Position - startPoseA.Position; Vector3F linearVelocityB = targetPoseB.Position - startPoseB.Position; Vector3F linearVelocityRelative = linearVelocityA - linearVelocityB; // Abort if relative movement is zero. if (Numeric.IsZero(linearVelocityRelative.Length)) { return(1); } var distanceAlgorithm = collisionDetection.AlgorithmMatrix[objectA, objectB]; // Use temporary test objects. var testGeometricObjectA = TestGeometricObject.Create(); testGeometricObjectA.Shape = geometricObjectA.Shape; testGeometricObjectA.Scale = geometricObjectA.Scale; testGeometricObjectA.Pose = startPoseA; var testGeometricObjectB = TestGeometricObject.Create(); testGeometricObjectB.Shape = geometricObjectB.Shape; testGeometricObjectB.Scale = geometricObjectB.Scale; testGeometricObjectB.Pose = startPoseB; var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain(); testCollisionObjectA.SetInternal(objectA, testGeometricObjectA); var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain(); testCollisionObjectB.SetInternal(objectB, testGeometricObjectB); var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB); try { distanceAlgorithm.UpdateClosestPoints(testContactSet, 0); if (testContactSet.Count < 0) { // No closest-distance result. --> Abort. return(1); } Vector3F normal = testContactSet[0].Normal; float distance = -testContactSet[0].PenetrationDepth; float λ = 0; float λPrevious = 0; for (int i = 0; i < MaxNumberOfIterations && distance > 0; i++) { // |v∙n| float velocityProjected = Vector3F.Dot(linearVelocityRelative, normal); // Abort for separating objects. if (Numeric.IsLess(velocityProjected, 0)) { break; } // Increase TOI. float μ = (distance + allowedPenetration) / velocityProjected; λ = λ + μ; if (λ < 0 || λ > 1) { break; } Debug.Assert(λPrevious < λ); if (λ <= λPrevious) { break; } // Get new interpolated poses - only positions are changed. Vector3F positionA = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position); testGeometricObjectA.Pose = new Pose(positionA, startPoseA.Orientation); Vector3F positionB = startPoseB.Position + λ * (targetPoseB.Position - startPoseB.Position); testGeometricObjectB.Pose = new Pose(positionB, startPoseB.Orientation); // Get new closest point distance. distanceAlgorithm.UpdateClosestPoints(testContactSet, 0); if (testContactSet.Count == 0) { break; } normal = testContactSet[0].Normal; distance = -testContactSet[0].PenetrationDepth; λPrevious = λ; } if (testContactSet.HaveContact && λ > 0 && λ < 1 && testContactSet.Count > 0) { return(λ); // We already have a contact that we could use. // result.Contact = testContactSet[0]; } } finally { // Recycle temporary objects. testContactSet.Recycle(true); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA); ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB); testGeometricObjectA.Recycle(); testGeometricObjectB.Recycle(); } return(1); }
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 } }