C# CollisionObject.SetInternal方法代码示例

//.........这里部分代码省略.........
            // (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
        }

    // 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.
//.........这里部分代码省略.........

    // 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);
      }
//.........这里部分代码省略.........

    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);
//.........这里部分代码省略.........