예제 #1
0
        /// <summary>
        /// Updates the contact geometry of a contact set.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        /// <param name="deltaTime">
        /// The time step in seconds. (The simulation time that has elapsed since the last time that an 
        /// Update-method was called.)
        /// </param>
        /// <param name="contactPositionTolerance">The contact position tolerance.</param>
        /// <remarks>
        /// <para>
        /// The objects can still move relative to each other. This method updates the contact 
        /// information if the objects have moved. The <see cref="Contact.Lifetime"/> of persistent 
        /// contacts is increased. Invalid contacts are removed. Closest point pairs will be removed if 
        /// they start touching. Penetrating or touching contacts are removed if the objects have moved 
        /// more than <paramref name="contactPositionTolerance"/> or the contacts have separated.
        /// </para>
        /// <para>
        /// Note: Only the info of the cached contacts is updated. New contacts are not discovered in
        /// this method.
        /// </para>
        /// </remarks>
        internal static void UpdateContacts(ContactSet contactSet, float deltaTime, float contactPositionTolerance)
        {
            Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");

              int numberOfContacts = contactSet.Count;
              if (numberOfContacts == 0)
            return;

              if (contactSet.ObjectA.Changed || contactSet.ObjectB.Changed)
              {
            // Objects have moved.
            for (int i = numberOfContacts - 1; i >= 0; i--)   // Reverse loop, because contacts might be removed.
            {
              Contact contact = contactSet[i];

              // Increase Lifetime.
              contact.Lifetime += deltaTime;

              // Update all contacts and remove invalid contacts.
              bool shouldRemove = UpdateContact(contactSet, contact, contactPositionTolerance);
              if (shouldRemove)
              {
            contactSet.RemoveAt(i);
            contact.Recycle();
              }
            }
              }
              else
              {
            // Nothing has moved.
            for (int i = 0; i < numberOfContacts; i++)
            {
              // Increase Lifetime.
              contactSet[i].Lifetime += deltaTime;
            }
              }
        }
예제 #2
0
 /// <summary>
 /// Removes separated contacts.
 /// </summary>
 /// <param name="contactSet">The contact set.</param>
 internal static void RemoveSeparatedContacts(ContactSet contactSet)
 {
     for (int i = contactSet.Count - 1; i >= 0; i--)
       {
     Contact contact = contactSet[i];
     if (contact.PenetrationDepth < 0)
     {
       contactSet.RemoveAt(i);
       contact.Recycle();
     }
       }
 }
예제 #3
0
        public static void RemoveBadContacts(ContactSet contactSet, Vector3F normal, float minDotProduct)
        {
            for (int i = contactSet.Count - 1; i >= 0; i--)
              {
            Contact contact = contactSet[i];
            Vector3F contactNormal = contact.Normal;

            // float dot = Vector3F.Dot(contactNormal, normal);

            // ----- Optimized version:
            float dot = contactNormal.X * normal.X + contactNormal.Y * normal.Y + contactNormal.Z * normal.Z;

            if (dot < minDotProduct)
            {
              contactSet.RemoveAt(i);
              contact.Recycle();
            }
              }
        }
        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
            }
        }
예제 #5
0
        /// <summary>
        /// Removes 1 contact.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        /// <remarks>
        /// <c>contactSet[0]</c> is not changed. <c>contactSet[1]</c> to <c>contactSet[4]</c> are
        /// tested. The contacts with the largest area are kept. The other one is removed.
        /// </remarks>
        private static void Reduce(ContactSet contactSet)
        {
            Debug.Assert(contactSet.Count > 4);

              // Remember: The magnitude of the cross product of two vectors is equal to the area of the
              // defined parallelogram. The parallelogram is twice as large as the triangle area defined
              // between the three points.
              Vector3F edge12 = contactSet[2].Position - contactSet[1].Position;
              Vector3F edge13 = contactSet[3].Position - contactSet[1].Position;
              Vector3F edge14 = contactSet[4].Position - contactSet[1].Position;
              Vector3F edge23 = contactSet[3].Position - contactSet[2].Position;
              Vector3F edge24 = contactSet[4].Position - contactSet[2].Position;

              // Check 4 parallelograms.
              float area = Vector3F.Cross(edge12, edge13).LengthSquared;
              float maxArea = area;
              int contactToDelete = 4;

              area = Vector3F.Cross(edge12, edge14).LengthSquared;
              if (area > maxArea)
              {
            maxArea = area;
            contactToDelete = 3;
              }

              area = Vector3F.Cross(edge13, edge14).LengthSquared;
              if (area > maxArea)
              {
            maxArea = area;
            contactToDelete = 2;
              }

              area = Vector3F.Cross(edge23, edge24).LengthSquared;
              if (area > maxArea)
              {
            // maxArea = area;
            contactToDelete = 1;
              }

              contactSet[contactToDelete].Recycle();

              // Remove 1 contact by moving the last contact in the contact set to its index.
              // Unless the contact to delete is already the last one.
              int maxIndex = contactSet.Count - 1;
              if (contactToDelete != maxIndex)
            contactSet[contactToDelete] = contactSet[maxIndex];

              contactSet.RemoveAt(maxIndex);
        }
예제 #6
0
    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
      }
    }