/// <summary> /// Updates the contact information in the given contact set. /// </summary> /// <param name="contactSet">The contact set containing the last known contacts.</param> /// <param name="deltaTime"> /// The time step size in seconds. (The elapsed simulation time since /// <see cref="UpdateClosestPoints"/> or <see cref="UpdateContacts"/> was last called for this /// contact set.) /// </param> /// <remarks> /// If two objects move, the contact information will usually change and has to be updated. /// Using the contact set containing the last known contacts, this method can compute the new /// contacts faster than <see cref="GetContacts"/> if the poses of the objects haven't changed /// drastically. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="contactSet"/> is <see langword="null"/>. /// </exception> public void UpdateContacts(ContactSet contactSet, float deltaTime) { if (contactSet == null) { throw new ArgumentNullException("contactSet"); } // Broad phase AABB check and collision filtering if (HaveAabbContact(contactSet.ObjectA, contactSet.ObjectB)) { // Narrow phase AlgorithmMatrix[contactSet].UpdateContacts(contactSet, deltaTime); } else { foreach (var contact in contactSet) { contact.Recycle(); } contactSet.Clear(); contactSet.HaveContact = false; } }
/// <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 = Vector3F.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 = Matrix33F.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 = Vector3F.Cross(axis1, normal); var axis2Local = pose.ToLocalDirection(axis2); var rotation2 = Matrix33F.CreateRotation(axis2Local, -angle); testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2); // Compute next contacts. testMethod(testContactSet); // Invert rotation2. rotation2.Transpose(); 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(); }
/// <summary> /// Updates the contact geometry for a single contact. /// </summary> /// <param name="contactSet">The contact set.</param> /// <param name="contact">The contact to be updated.</param> /// <param name="contactPositionTolerance">The contact position tolerance.</param> /// <returns> /// <see langword="true"/> if the contact is invalid and should be removed. /// </returns> private static bool UpdateContact(ContactSet contactSet, Contact contact, float contactPositionTolerance) { Pose poseA = contactSet.ObjectA.GeometricObject.Pose; Pose poseB = contactSet.ObjectB.GeometricObject.Pose; // Get local positions in world space. //Vector3F positionA = poseA.ToWorldPosition(contact.PositionALocal); //Vector3F positionB = poseB.ToWorldPosition(contact.PositionBLocal); // ----- Optimized version: Vector3F positionALocal = contact.PositionALocal; Vector3F positionA; positionA.X = poseA.Orientation.M00 * positionALocal.X + poseA.Orientation.M01 * positionALocal.Y + poseA.Orientation.M02 * positionALocal.Z + poseA.Position.X; positionA.Y = poseA.Orientation.M10 * positionALocal.X + poseA.Orientation.M11 * positionALocal.Y + poseA.Orientation.M12 * positionALocal.Z + poseA.Position.Y; positionA.Z = poseA.Orientation.M20 * positionALocal.X + poseA.Orientation.M21 * positionALocal.Y + poseA.Orientation.M22 * positionALocal.Z + poseA.Position.Z; Vector3F positionBLocal = contact.PositionBLocal; Vector3F positionB; positionB.X = poseB.Orientation.M00 * positionBLocal.X + poseB.Orientation.M01 * positionBLocal.Y + poseB.Orientation.M02 * positionBLocal.Z + poseB.Position.X; positionB.Y = poseB.Orientation.M10 * positionBLocal.X + poseB.Orientation.M11 * positionBLocal.Y + poseB.Orientation.M12 * positionBLocal.Z + poseB.Position.Y; positionB.Z = poseB.Orientation.M20 * positionBLocal.X + poseB.Orientation.M21 * positionBLocal.Y + poseB.Orientation.M22 * positionBLocal.Z + poseB.Position.Z; // Update Position. contact.Position = (positionA + positionB) / 2; // Update contacts and closest points differently: if (contact.PenetrationDepth >= 0) { // ----- Contact. Vector3F bToA = positionA - positionB; // Vector from contact on A to contact on B if (!contact.IsRayHit) { // ----- Normal contact. // Update penetration depth: Difference of world position projected onto normal. //contact.PenetrationDepth = Vector3F.Dot(bToA, contact.Normal); // ----- Optimized version: Vector3F contactNormal = contact.Normal; contact.PenetrationDepth = bToA.X * contactNormal.X + bToA.Y * contactNormal.Y + bToA.Z * contactNormal.Z; } else { // ----- Ray hit. // Update penetration depth: Contact position to ray origin projected onto ray direction. // Get ray. Only one shape is a ray because ray vs. ray do normally not collide. RayShape ray = contactSet.ObjectA.GeometricObject.Shape as RayShape; float rayScale = contactSet.ObjectA.GeometricObject.Scale.X; // Non-uniformly scaled rays are not support, so we only need Scale.X! Vector3F hitPositionLocal; // Hit position in local space of ray. if (ray != null) { hitPositionLocal = poseA.ToLocalPosition(contact.Position); } else { // The other object must be the ray. ray = contactSet.ObjectB.GeometricObject.Shape as RayShape; rayScale = contactSet.ObjectB.GeometricObject.Scale.X; // Non-uniformly scaled rays are not support, so we only need Scale.X! hitPositionLocal = poseB.ToLocalPosition(contact.Position); } // Now, we have found the ray, unless there is a composite shape with a child ray - which // is not supported. if (ray != null) { contact.PenetrationDepth = Vector3F.Dot(hitPositionLocal - ray.Origin * rayScale, ray.Direction); // If the new penetration depth is negative or greater than the ray length, // the objects have separated along the ray direction. if (contact.PenetrationDepth < 0 || contact.PenetrationDepth > ray.Length * rayScale) { return(true); } } } // Remove points with negative penetration depth. if (contact.PenetrationDepth < 0) { return(true); } // Check drift. float driftSquared; if (contact.IsRayHit) { // For ray casts: Remove contact if movement in any direction is too large. driftSquared = bToA.LengthSquared; } else { // For contacts: Remove contacts if horizontal movement (perpendicular to contact normal) // is too large. driftSquared = (bToA - contact.Normal * contact.PenetrationDepth).LengthSquared; } // Remove contact if drift is too large. return(driftSquared > contactPositionTolerance * contactPositionTolerance); } else { // ----- Closest point pair. // Update distance. Since we do not check the geometric objects, the new distance // could be a separation or a penetration. We assume it is a separation and // use a "-" sign. // We have no problem if we are wrong and this is actually a penetration because this // contact is automatically updated or removed when new contacts are computed in // the narrow phase. Vector3F aToB = positionB - positionA; // Vector from contact on A to contact on B contact.PenetrationDepth = -aToB.Length; // If points moved into contact, remove this pair, because we don't have a valid // contact normal. if (Numeric.IsZero(contact.PenetrationDepth)) { return(true); } // Update normal. contact.Normal = aToB.Normalized; return(false); } }
/// <summary> /// Reduces the number of contacts in the contact set to 1 contact. /// </summary> /// <param name="contactSet"> /// The contact set. One shape in the contact set must be a <see cref="RayShape"/>! /// </param> /// <remarks> /// The best ray hit is kept. /// </remarks> internal static void ReduceRayHits(ContactSet contactSet) { Debug.Assert( contactSet.ObjectA.GeometricObject.Shape is RayShape || contactSet.ObjectB.GeometricObject.Shape is RayShape, "ReduceRayHits was called for a contact set without a ray."); // For separated contacts keep the contact with the smallest separation. // If we have contact, keep the contact with the SMALLEST penetration (= shortest ray length) // and remove all invalid contacts (with separation). bool haveContact = contactSet.HaveContact; float bestPenetrationDepth = haveContact ? float.PositiveInfinity : float.NegativeInfinity; Contact bestContact = null; int numberOfContacts = contactSet.Count; for (int i = 0; i < numberOfContacts; i++) { Contact contact = contactSet[i]; float penetrationDepth = contact.PenetrationDepth; if (haveContact) { // Search for positive and smallest penetration depth. if (penetrationDepth >= 0 && penetrationDepth < bestPenetrationDepth) { bestContact = contact; bestPenetrationDepth = penetrationDepth; } } else { // Search for negative and largest penetration depth (Separation!) Debug.Assert(penetrationDepth < 0, "HaveContact is false, but contact shows penetration."); if (penetrationDepth > bestPenetrationDepth) { bestContact = contact; bestPenetrationDepth = penetrationDepth; } } } // Keep best contact. // Note: In some situations HaveContact is true, but the contact set does not contains any // contacts with penetration. This happen, for example, when testing a ray inside a triangle // mesh. The TriangleMeshAlgorithm automatically filters contacts with bad normals. // When HaveContact is false, we should always have a contact (closest point). // Throw away other contacts. foreach (var contact in contactSet) { if (contact != bestContact) { contact.Recycle(); } } contactSet.Clear(); if (bestContact != null) { contactSet.Add(bestContact); } }