Ejemplo n.º 1
0
        /// <summary>
        /// Computes the contacts between two <see cref="CollisionObject"/>s.
        /// </summary>
        /// <param name="objectA">The first collision object.</param>
        /// <param name="objectB">The second collision object.</param>
        /// <returns>
        /// A <see cref="ContactSet"/> describing the contact information if <paramref name="objectA"/>
        /// and <paramref name="objectB"/> are intersecting; otherwise, <see langword="null"/> if the
        /// objects are separated.
        /// </returns>
        /// <exception cref="ArgumentNullException">
        /// <paramref name="objectA"/> is <see langword="null"/>.
        /// </exception>
        /// <exception cref="ArgumentNullException">
        /// <paramref name="objectB"/> is <see langword="null"/>.
        /// </exception>
        public ContactSet GetContacts(CollisionObject objectA, CollisionObject objectB)
        {
            // Broad phase AABB check and collision filtering
            if (HaveAabbContact(objectA, objectB) == false)
            {
                return(null);
            }

            // Narrow phase
            ContactSet contactSet = AlgorithmMatrix[objectA, objectB].GetContacts(objectA, objectB);

            Debug.Assert(contactSet != null, "CollisionAlgorithm.GetContacts should always return a ContactSet.");
            if (contactSet.HaveContact)
            {
                return(contactSet);
            }
            else
            {
                contactSet.Recycle();
                return(null);
            }
        }
Ejemplo n.º 2
0
        /// <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;
            }
        }
Ejemplo n.º 3
0
        /// <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();
        }
Ejemplo n.º 4
0
        /// <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.
            //Vector3 positionA = poseA.ToWorldPosition(contact.PositionALocal);
            //Vector3 positionB = poseB.ToWorldPosition(contact.PositionBLocal);

            // ----- Optimized version:
            Vector3 positionALocal = contact.PositionALocal;
            Vector3 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;
            Vector3 positionBLocal = contact.PositionBLocal;
            Vector3 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.
                Vector3 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 = Vector3.Dot(bToA, contact.Normal);

                    // ----- Optimized version:
                    Vector3 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!
                    Vector3  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 = Vector3.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.
                Vector3 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);
            }
        }
Ejemplo n.º 5
0
        /// <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);
            }
        }