Example #1
0
    public void Swapped()
    {
      CollisionObject a = new CollisionObject();
      CollisionObject b = new CollisionObject();

      ContactSet set = ContactSet.Create(a, b);
      set.Add(ContactHelper.CreateContact(set, new Vector3(1, 2, 3), new Vector3(0, 0, 1), 10, false));
      set.Add(ContactHelper.CreateContact(set, new Vector3(4, 5, 6), new Vector3(0, 0, 1), 10, false));
      set.Add(ContactHelper.CreateContact(set, new Vector3(7, 8, 9), new Vector3(0, 0, 1), 10, false));

      ContactSet swapped = set.Swapped;
      Assert.AreEqual(set.ObjectA, swapped.ObjectB);
      Assert.AreEqual(set.ObjectB, swapped.ObjectA);
      Assert.AreEqual(set[0].PositionALocal, swapped[0].PositionBLocal);
      Assert.AreEqual(set[1].Normal, -swapped[1].Normal);
    }
Example #2
0
        public void TestContainment()
        {
            PlaneConvexAlgorithm algo = new PlaneConvexAlgorithm(new CollisionDetection());

            CollisionObject a = new CollisionObject(new GeometricObject
            {
                Shape = new BoxShape(1, 2, 3),
                Pose  = new Pose(new Vector3(0, -1, 0)),
            });
            CollisionObject b = new CollisionObject(new GeometricObject
            {
                Shape = new PlaneShape(new Vector3(0, 1, 0), 0),
            });

            Assert.AreEqual(true, algo.HaveContact(a, b));
            Assert.AreEqual(true, algo.HaveContact(b, a));

            // Test contact set update.
            ContactSet cs = ContactSet.Create(a, b);

            cs.Add(Contact.Create());
            algo.UpdateContacts(cs, 0);
            Assert.AreEqual(1, cs.Count);
            Assert.AreEqual(2, cs[0].PenetrationDepth);
        }
Example #3
0
        public void Merge2()
        {
            ContactSet set1 = ContactSet.Create(new CollisionObject(), new CollisionObject());
            ContactSet set2 = ContactSet.Create(set1.ObjectA, set1.ObjectB);

            Contact contact = Contact.Create();

            contact.Position = new Vector3(1, 2, 3);
            set2.Add(contact);

            ContactHelper.Merge(set2, set1, CollisionQueryType.Contacts, 0.01f);
            Assert.AreEqual(1, set2.Count);

            contact          = Contact.Create();
            contact.Position = new Vector3(1, 2, 3);
            set1.Add(contact);

            contact          = Contact.Create();
            contact.Position = new Vector3(2, 2, 3);
            set1.Add(contact);

            contact          = Contact.Create();
            contact.Position = new Vector3(3, 2, 3);
            set1.Add(contact);

            ContactHelper.Merge(set2, set1, CollisionQueryType.Contacts, 0.01f);
            Assert.AreEqual(3, set2.Count);
        }
Example #4
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            if (type == CollisionQueryType.ClosestPoints)
            {
                // Closest point queries.
                _closestPointsAlgorithm.ComputeCollision(contactSet, type);
                if (contactSet.HaveContact)
                {
                    // Penetration.

                    // Remember the closest point info in case we run into troubles.
                    // Get the last contact. We assume that this is the newest. In most cases there will
                    // only be one contact in the contact set.
                    Contact fallbackContact = (contactSet.Count > 0)
                                    ? contactSet[contactSet.Count - 1]
                                    : null;

                    // Call the contact query algorithm.
                    _contactAlgorithm.ComputeCollision(contactSet, CollisionQueryType.Contacts);

                    if (!contactSet.HaveContact)
                    {
                        // Problem!
                        // The closest-point algorithm reported contact. The contact algorithm didn't find a contact.
                        // This can happen, for example, because of numerical inaccuracies in GJK vs. MPR.
                        // Keep the result of the closest-point computation, but decrease the penetration depth
                        // to indicate separation.
                        if (fallbackContact != null)
                        {
                            Debug.Assert(fallbackContact.PenetrationDepth == 0);
                            fallbackContact.PenetrationDepth = -Math.Min(10 * Numeric.EpsilonF, CollisionDetection.Epsilon);

                            foreach (var contact in contactSet)
                            {
                                if (contact != fallbackContact)
                                {
                                    contact.Recycle();
                                }
                            }

                            contactSet.Clear();
                            contactSet.Add(fallbackContact);
                        }

                        contactSet.HaveContact = false;
                    }
                }
            }
            else
            {
                // Boolean or contact queries.
                _contactAlgorithm.ComputeCollision(contactSet, type);
            }
        }
Example #5
0
        public void TestMethods()
        {
            NoCollisionAlgorithm algo = new NoCollisionAlgorithm(new CollisionDetection());

            CollisionObject a = new CollisionObject {
                GeometricObject = new GeometricObject {
                    Shape = new SphereShape(1)
                }
            };
            CollisionObject b = new CollisionObject {
                GeometricObject = new GeometricObject {
                    Shape = new SphereShape(2)
                }
            };

            Assert.AreEqual(a, algo.GetClosestPoints(a, b).ObjectA);
            Assert.AreEqual(b, algo.GetClosestPoints(a, b).ObjectB);
            Assert.AreEqual(0, algo.GetClosestPoints(a, b).Count);

            Assert.AreEqual(a, algo.GetContacts(a, b).ObjectA);
            Assert.AreEqual(b, algo.GetContacts(a, b).ObjectB);
            Assert.AreEqual(0, algo.GetContacts(a, b).Count);

            Assert.AreEqual(false, algo.HaveContact(a, b));

            ContactSet cs = ContactSet.Create(a, b);

            algo.UpdateClosestPoints(cs, 0);
            Assert.AreEqual(a, cs.ObjectA);
            Assert.AreEqual(b, cs.ObjectB);
            Assert.AreEqual(0, cs.Count);

            cs = ContactSet.Create(a, b);
            cs.Add(Contact.Create());
            algo.UpdateContacts(cs, 0);
            Assert.AreEqual(a, cs.ObjectA);
            Assert.AreEqual(b, cs.ObjectB);
            Assert.AreEqual(0, cs.Count);
        }
        public void TestNoContact()
        {
            SphereSphereAlgorithm algo = new SphereSphereAlgorithm(new CollisionDetection());

            CollisionObject objectA = new CollisionObject();

            ((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(1);
            CollisionObject objectB = new CollisionObject();

            ((GeometricObject)objectB.GeometricObject).Shape = new SphereShape(0.5f);

            ((GeometricObject)objectA.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
            ((GeometricObject)objectB.GeometricObject).Pose = new Pose(new Vector3F(1.6f, 0, 0));

            ContactSet cs = ContactSet.Create(objectA, objectB);

            cs.Add(ContactHelper.CreateContact(cs, Vector3F.Zero, Vector3F.UnitX, 0, false));

            algo.UpdateContacts(cs, 0);
            Assert.AreEqual(objectA, cs.ObjectA);
            Assert.AreEqual(objectB, cs.ObjectB);
            Assert.AreEqual(0, cs.Count);
        }
        public void UpdateClosestPoints()
        {
            ContactSet set = ContactSet.Create(_objectA, _objectB);

            _collisionDetection.UpdateClosestPoints(set, 0);
            Assert.AreEqual(1, set.Count);
            Assert.AreEqual(_objectA, set.ObjectA);
            Assert.AreEqual(_objectB, set.ObjectB);

            set = ContactSet.Create(_objectA, _objectC);
            set.Add(ContactHelper.CreateContact(set, new Vector3F(1, 0, 0), new Vector3F(1, 0, 0), -10, false));
            _collisionDetection.UpdateClosestPoints(set, 0);
            Assert.AreEqual(1, set.Count);
            Assert.AreEqual(_objectA, set.ObjectA);
            Assert.AreEqual(_objectC, set.ObjectB);

            _collisionDetection.CollisionFilter = new CollisionFilter();
            ((CollisionFilter)_collisionDetection.CollisionFilter).Set(_objectA, _objectB, false);
            set = ContactSet.Create(_objectA, _objectB);
            _collisionDetection.UpdateClosestPoints(set, 0);
            Assert.AreEqual(1, set.Count);
            Assert.AreEqual(_objectA, set.ObjectA);
            Assert.AreEqual(_objectB, set.ObjectB);
        }
Example #8
0
            public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
            {
                contactSet.Clear();
                contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(1, 2, 3), Vector3F.UnitZ, 1, false));

                if (type == CollisionQueryType.Contacts)
                  contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(2, 2, 3), Vector3F.UnitZ, 1.2f, false));

                contactSet.HaveContact = true;
            }
Example #9
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 = 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();
        }
Example #10
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);
        }
Example #11
0
        /// <summary>
        /// Reduces the number of contacts in the contact set to 1 contact.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        /// <remarks>
        /// The contact with the biggest penetration depth is kept.
        /// </remarks>
        internal static void ReduceClosestPoints(ContactSet contactSet)
        {
            // Reduce to 1 contact.
              int numberOfContacts = contactSet.Count;
              if (numberOfContacts > 1)
              {
            // Keep the contact with the deepest penetration depth.
            Contact bestContact = contactSet[0];
            for (int i = 1; i < numberOfContacts; i++)
            {
              if (contactSet[i].PenetrationDepth > bestContact.PenetrationDepth)
              {
            bestContact = contactSet[i];
              }
            }

            // Throw away other contacts.
            foreach (var contact in contactSet)
              if (contact != bestContact)
            contact.Recycle();

            contactSet.Clear();
            contactSet.Add(bestContact);
              }

              Debug.Assert(contactSet.Count == 0 || contactSet.Count == 1);

              // If we HaveContact but the contact shows a separation, delete contact.
              // This can happen for TriangleMesh vs. TriangleMesh because the triangle mesh algorithm
              // filters contacts if they have a bad normal. It can happen that all contacts are filtered
              // and only a separated contact remains in the contact set.
              if (contactSet.HaveContact && contactSet.Count > 0 && contactSet[0].PenetrationDepth < 0)
              {
            contactSet[0].Recycle();
            contactSet.Clear();
              }
        }
Example #12
0
        public static void Merge(ContactSet contactSet, Contact newContact, CollisionQueryType type, float contactPositionTolerance)
        {
            Debug.Assert(contactSet != null);
              Debug.Assert(newContact != null);

              Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
              Debug.Assert(type == CollisionQueryType.ClosestPoints || type == CollisionQueryType.Contacts);

              if (type == CollisionQueryType.Contacts && newContact.PenetrationDepth < 0)
            return; // Do not merge separated contacts.

              // ----- Simplest case: Contact set is empty.
              if (contactSet.Count == 0)
              {
            // Simply add the new contact.
            contactSet.Add(newContact);
            return;
              }

              // ----- Try to merge with nearest old contact.
              bool merged = TryMergeWithNearestContact(contactSet, newContact, contactPositionTolerance, true);
              if (merged)
              {
            newContact.Recycle();
            return;
              }

              // ----- Default: Add the new contact.
              contactSet.Add(newContact);
        }
Example #13
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            if (type == CollisionQueryType.ClosestPoints)
              {
            // Closest point queries.
            _closestPointsAlgorithm.ComputeCollision(contactSet, type);
            if (contactSet.HaveContact)
            {
              // Penetration.

              // Remember the closest point info in case we run into troubles.
              // Get the last contact. We assume that this is the newest. In most cases there will
              // only be one contact in the contact set.
              Contact fallbackContact = (contactSet.Count > 0)
                                    ? contactSet[contactSet.Count - 1]
                                    : null;

              // Call the contact query algorithm.
              _contactAlgorithm.ComputeCollision(contactSet, CollisionQueryType.Contacts);

              if (!contactSet.HaveContact)
              {
            // Problem!
            // The closest-point algorithm reported contact. The contact algorithm didn't find a contact.
            // This can happen, for example, because of numerical inaccuracies in GJK vs. MPR.
            // Keep the result of the closest-point computation, but decrease the penetration depth
            // to indicate separation.
            if (fallbackContact != null)
            {
              Debug.Assert(fallbackContact.PenetrationDepth == 0);
              fallbackContact.PenetrationDepth = -Math.Min(10 * Numeric.EpsilonF, CollisionDetection.Epsilon);

              foreach (var contact in contactSet)
                if (contact != fallbackContact)
                  contact.Recycle();

              contactSet.Clear();
              contactSet.Add(fallbackContact);
            }

            contactSet.HaveContact = false;
              }
            }
              }
              else
              {
            // Boolean or contact queries.
            _contactAlgorithm.ComputeCollision(contactSet, type);
              }
        }