// 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);
            }
        }
예제 #2
0
        // 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
        }
        // 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
        }
예제 #4
0
    // 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);
      }
    }
예제 #5
0
    // 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);
      }
    }
예제 #6
0
        // 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);
            }
        }
        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
            }
        }
예제 #8
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
      }
    }