Ejemplo n.º 1
0
        /// <summary>
        /// Computes the collision between line vs. line.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        /// <param name="type">The type of collision query.</param>
        private void ComputeLineVsLine(ContactSet contactSet, CollisionQueryType type)
        {
            IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
              IGeometricObject objectB = contactSet.ObjectB.GeometricObject;

              Debug.Assert(objectA.Shape is LineShape && objectB.Shape is LineShape, "LineAlgorithm.ComputeLineVsLine should only be called for 2 line shapes.");
              Debug.Assert(contactSet.Count <= 1, "Two lines should have at max 1 contact point.");

              // Get transformations.
              Vector3F scaleA = objectA.Scale;
              Vector3F scaleB = objectB.Scale;
              Pose poseA = objectA.Pose;
              Pose poseB = objectB.Pose;

              // Create two line objects in world space.
              var lineA = new Line((LineShape)objectA.Shape);
              lineA.Scale(ref scaleA);
              lineA.ToWorld(ref poseA);

              var lineB = new Line((LineShape)objectB.Shape);
              lineB.Scale(ref scaleB);
              lineB.ToWorld(ref poseB);

              // Get closest points.
              Vector3F pointA;
              Vector3F pointB;
              contactSet.HaveContact = GeometryHelper.GetClosestPoints(lineA, lineB, out pointA, out pointB);

              if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
              {
            // HaveContact queries can exit here.
            // GetContacts queries can exit here if we don't have a contact.
            return;
              }

              // Create contact information.
              Vector3F position = (pointA + pointB) / 2;
              Vector3F normal = pointB - pointA;
              float length = normal.Length;
              if (Numeric.IsZero(length))
              {
            // Create normal from cross product of both lines.
            normal = Vector3F.Cross(lineA.Direction, lineB.Direction);
            if (!normal.TryNormalize())
              normal = Vector3F.UnitY;
              }
              else
              {
            // Normalize vector
            normal = normal / length;
              }

              Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -length, false);
              ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
        }
Ejemplo n.º 2
0
    public VisualizeContactSet(Microsoft.Xna.Framework.Game game)
      : base(game)
    {
      SampleFramework.IsMouseVisible = false;
      GraphicsScreen.ClearBackground = true;
      SetCamera(new Vector3F(0, 1, 10), 0, 0);

      CreateObjects();

      _contactSet = _collisionDetection.GetContacts(_objectA, _objectB);
    }
Ejemplo n.º 3
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            if (type == CollisionQueryType.ClosestPoints)
              {
            // Closest point queries.
            _closestPointsAlgorithm.ComputeCollision(contactSet, type);
            if (contactSet.HaveContact)
            {
              // Penetration.

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

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

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

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

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

            contactSet.HaveContact = false;
              }
            }
              }
              else
              {
            // Boolean or contact queries.
            _contactAlgorithm.ComputeCollision(contactSet, type);
              }
        }
Ejemplo n.º 4
0
 public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
 {
     bool isLineA = contactSet.ObjectA.GeometricObject.Shape is LineShape;
       bool isLineB = contactSet.ObjectB.GeometricObject.Shape is LineShape;
       if (isLineA && isLineB)
       {
     ComputeLineVsLine(contactSet, type);
       }
       else if (isLineA || isLineB)
       {
     ComputeLineVsOther(contactSet, type, isLineA);
       }
       else
       {
     throw new ArgumentException("The contact set must contain a line.", "contactSet");
       }
 }
Ejemplo n.º 5
0
        // TODO: Make the max. number of contact points configurable or experiment to find out the optimal number.
        // TODO: We could also keep the convex hull of contact points in the 2D plane defined by the (average) normal vector.
        /// <summary>
        /// Filters the specified contact set.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        public void Filter(ContactSet contactSet)
        {
            if (contactSet == null)
            return;

              int numberOfContacts = contactSet.Count;

              // Nothing to do if we have 4 or less contacts.
              if (numberOfContacts <= 4)
            return;

              // First of all we keep the best contact: This is the contact with the deepest penetration.
              // Note: We could also argue that the newest contact is the best, but then following
              // problem occurs: Two composite objects touch with 5 contacts. 1 is removed. In
              // the next step the same 5 contacts are found. The one that was removed in the last
              // frame is now the newest contact, so another contact is removed. --> 1 Contact will
              // always toggle and is not persistent.

              // Find the deepest contact
              int deepestContactIndex = 0;
              float deepestPenetrationDepth = contactSet[0].PenetrationDepth;
              for (int i = 1; i < numberOfContacts; i++)
              {
            Contact contact = contactSet[i];
            if (contact.PenetrationDepth > deepestPenetrationDepth)
            {
              deepestPenetrationDepth = contact.PenetrationDepth;
              deepestContactIndex = i;
            }
              }

              // Move deepest contact to index 0.
              if (deepestContactIndex != 0)
              {
            Contact deepest = contactSet[deepestContactIndex];
            contactSet[deepestContactIndex] = contactSet[0];
            contactSet[0] = deepest;
              }

              // Iteratively, remove single contacts until 4 are left.
              while (contactSet.Count > 4)
            Reduce(contactSet);
        }
Ejemplo n.º 6
0
        private static void CheckResult(ContactSet contactSet, bool isClosestPointResult)
        {
            if (isClosestPointResult && contactSet.Count > 1)
            {
                throw new GeometryException("Closest point query result contains more than one contact.");
            }

            if (contactSet.HaveContact)
            {
                if (contactSet.Any(contact => contact.PenetrationDepth < 0))
                {
                    throw new GeometryException("HaveContact is true but the contactSet contains a separated contact.");
                }

                if (contactSet.ObjectA.GeometricObject.Shape is RayShape || contactSet.ObjectB.GeometricObject.Shape is RayShape)
                {
                    if (contactSet.Count > 1)
                    {
                        throw new GeometryException("Ray casts must not contain more than 1 contact.");
                    }

                    if (contactSet.Any(contact => !contact.IsRayHit))
                    {
                        throw new GeometryException("IsRayHit is not set for a ray hit.");
                    }
                }
            }

            if (isClosestPointResult == false && contactSet.HaveContact == false && contactSet.Count > 0)
            {
                throw new GeometryException("A contact query returned contact details for a separated pair.");
            }

            if (contactSet.HaveContact == false && contactSet.Any(contact => contact.PenetrationDepth >= 0))
            {
                throw new GeometryException("HaveContact is false but the contactSet contains a touching/penetrating contact.");
            }
        }
        public void TestNoContact()
        {
            SphereSphereAlgorithm algo = new SphereSphereAlgorithm(new CollisionDetection());

            CollisionObject objectA = new CollisionObject();

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

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

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

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

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

            algo.UpdateContacts(cs, 0);
            Assert.AreEqual(objectA, cs.ObjectA);
            Assert.AreEqual(objectB, cs.ObjectB);
            Assert.AreEqual(0, cs.Count);
        }
        public void TestZeroSphere()
        {
            SphereSphereAlgorithm algo = new SphereSphereAlgorithm(new CollisionDetection());

            CollisionObject objectA = new CollisionObject();

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

            ((GeometricObject)objectB.GeometricObject).Shape = new SphereShape(0);

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

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

            algo.UpdateContacts(cs, 0);

            Assert.AreEqual(1, cs.Count);
            Assert.AreEqual(new Vector3F(0, 0, 1), cs[0].Position);
            //Assert.AreEqual(new Vector3F(0, 0, 1), cs.Contacts[0].Normal);
            Assert.IsTrue(Numeric.AreEqual(0, cs[0].PenetrationDepth));
        }
        public void TestInfiniteSphere()
        {
            SphereSphereAlgorithm algo = new SphereSphereAlgorithm(new CollisionDetection());

            CollisionObject objectA = new CollisionObject();

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

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

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

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

            algo.UpdateContacts(cs, 0);

            Assert.AreEqual(1, cs.Count);
            //Assert.AreEqual(new Vector3F(0, 1f, 0), cs.Contacts[0].Position);     // Undefined when a sphere is infinite.
            Assert.AreEqual(new Vector3F(0, 1, 0), cs[0].Normal);
            Assert.IsTrue(float.IsPositiveInfinity(cs[0].PenetrationDepth));
        }
Ejemplo n.º 10
0
        public void TestContainment()
        {
            PlaneSphereAlgorithm algo = new PlaneSphereAlgorithm(new CollisionDetection());

            CollisionObject objectA = new CollisionObject();

            ((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(1);
            ((GeometricObject)objectA.GeometricObject).Pose  = new Pose(new Vector3(0, -2, 0));

            CollisionObject objectB = new CollisionObject();

            ((GeometricObject)objectB.GeometricObject).Shape = new PlaneShape(new Vector3(0, 1, 0).Normalized, 0);
            ((GeometricObject)objectB.GeometricObject).Pose  = new Pose(new Vector3(0, 0, 0));

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

            algo.UpdateContacts(cs, 0);

            Assert.AreEqual(objectA, cs.ObjectA);
            Assert.AreEqual(objectB, cs.ObjectB);
            Assert.AreEqual(1, cs.Count);
            Assert.AreEqual(new Vector3(0, -1.5f, 0), cs[0].Position);
            Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, -1, 0), cs[0].Normal));
            Assert.IsTrue(Numeric.AreEqual(3, cs[0].PenetrationDepth));

            // Test swapped case:
            cs = ContactSet.Create(objectB, objectA);
            algo.UpdateContacts(cs, 0);

            Assert.AreEqual(objectB, cs.ObjectA);
            Assert.AreEqual(objectA, cs.ObjectB);
            Assert.AreEqual(1, cs.Count);
            Assert.AreEqual(new Vector3(0, -1.5f, 0), cs[0].Position);
            Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 1, 0), cs[0].Normal));
            Assert.IsTrue(Numeric.AreEqual(3, cs[0].PenetrationDepth));
        }
        public DumpContactSetSample(Microsoft.Xna.Framework.Game game)
            : base(game)
        {
            GraphicsScreen.ClearBackground = true;

            // Create two collision objects with triangle mesh shapes.
            var meshA  = new SphereShape(1).GetMesh(0.01f, 4);
            var shapeA = new TriangleMeshShape(meshA, true)
            {
                Partition = new CompressedAabbTree()
            };
            var poseA            = new Pose(new Vector3(-1, 0, 0), RandomHelper.Random.NextQuaternion());
            var collisionObjectA = new CollisionObject(new GeometricObject(shapeA, poseA));

            var meshB  = new BoxShape(0.2f, 2, 1f).GetMesh(0.01f, 4);
            var shapeB = new TriangleMeshShape(meshB, true)
            {
                Partition = new CompressedAabbTree()
            };
            var poseB            = new Pose(new Vector3(0.1f, 0, 0), RandomHelper.Random.NextQuaternion());
            var collisionObjectB = new CollisionObject(new GeometricObject(shapeB, poseB));

            // Explicitly create a contact set. (Normally you would get the contact set
            // from the collision domain...)
            var contactSet = ContactSet.Create(collisionObjectA, collisionObjectB);

            // Create a C# sample which visualizes the contact set.
            const string Filename = "DumpedContactSet001.cs";

            DumpContactSet(contactSet, Filename);

            GraphicsScreen.DebugRenderer2D.DrawText(
                "Contact set dumped into the file: " + Filename,
                new Vector2F(300, 300),
                Color.Black);
        }
Ejemplo n.º 12
0
        public void UpdateClosestPoints()
        {
            ContactSet set = ContactSet.Create(_objectA, _objectB);

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

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

            _collisionDetection.CollisionFilter = new CollisionFilter();
            ((CollisionFilter)_collisionDetection.CollisionFilter).Set(_objectA, _objectB, false);
            set = ContactSet.Create(_objectA, _objectB);
            _collisionDetection.UpdateClosestPoints(set, 0);
            Assert.AreEqual(1, set.Count);
            Assert.AreEqual(_objectA, set.ObjectA);
            Assert.AreEqual(_objectB, set.ObjectB);
        }
Ejemplo n.º 13
0
    public override void Update(GameTime gameTime)
    {
      _domain.EnableMultithreading = InputService.IsDown(Keys.Space);


      MessageBox.Show("Start");
      _deltaTime = 1 / 60f;
      for (int bla = 0; bla < 10000; bla++)
      {

      if (ClosestPointQueriesEnabled)
      {
        // Here we run closest point queries on all object pairs.
        // We compare the results with the contact queries.

        for (int i = 0; i < _domain.CollisionObjects.Count; i++)
        {
          for (int j = i + 1; j < _domain.CollisionObjects.Count; j++)
          {
            CollisionObject a = _domain.CollisionObjects[i];
            CollisionObject b = _domain.CollisionObjects[j];

            ContactSet closestPointQueryResult = _domain.CollisionDetection.GetClosestPoints(a, b);
            ContactSet contactSet = _domain.GetContacts(a, b);

            // Ignore height fields and rays.
            if (a.GeometricObject.Shape is HeightField || b.GeometricObject.Shape is HeightField)
              break;
            if (a.GeometricObject.Shape is RayShape || b.GeometricObject.Shape is RayShape)
              break;

            if (contactSet == null || !contactSet.HaveContact)
            {
              // No contact in contactSet
              if (closestPointQueryResult.HaveContact)
              {
                // Contact in closest point query. Results are inconsistent.
                if (closestPointQueryResult.Count > 0
                    && closestPointQueryResult[0].PenetrationDepth > 0.001f)
                  Debugger.Break();
              }
            }
            else if (!closestPointQueryResult.HaveContact)
            {
              // contact in contact query, but no contact in closest point query.
              // We allow a deviation within a small tolerance.
              if (closestPointQueryResult.Count > 0 && contactSet.Count > 0
                  && closestPointQueryResult[0].PenetrationDepth + contactSet[0].PenetrationDepth > 0.001f)
                Debugger.Break();
            }
          }
        }
      }

      // Reflect velocity if objects collide:
      // The collision domain contains a ContactSet for each pair of touching objects.
      foreach (var contactSet in _domain.ContactSets)
      {
        // Get the touching objects.
        var moA = (MovingGeometricObject)contactSet.ObjectA.GeometricObject;
        var moB = (MovingGeometricObject)contactSet.ObjectB.GeometricObject;

        // Reflect only at boundary objects.
        if (!(moA.Shape is PlaneShape) && !(moB.Shape is PlaneShape)
            && !(moA.Shape is HeightField) && !(moB.Shape is HeightField))
          continue;

        // Get normal vector. If objects are sensors, the contact set does not tell us
        // the right normal.
        Vector3 normal = Vector3.Zero;
        if (contactSet.Count > 0)
        {
          // Take normal from contact set.
          normal = contactSet[0].Normal;
        }
        else
        {
          // If we use Trigger CollisionObjects we do not have contacts. --> Reflect at
          // bounding planes.
          if (moA.Shape is PlaneShape)
            normal = ((PlaneShape)moA.Shape).Normal;
          else if (moB.Shape is PlaneShape)
            normal = -((PlaneShape)moB.Shape).Normal;
          else if (moA.Shape is HeightField)
            normal = Vector3.UnitY;
          else
            normal = -Vector3.UnitY;
        }
        //else if (moA.Shape is Plane || moB.Shape is Plane                       )
        //{
        //  // Use plane normal.
        //  IGeometricObject plane = moA.Shape is Plane ? moA : moB;
        //  normal = plane.Pose.ToWorldDirection(((Plane)plane.Shape).Normal);
        //  if (moB == plane)
        //    normal = -normal;
        //}
        //else if (moA.Shape is HeightField || moB.Shape is HeightField)
        //{
        //  // Use up-vector for height field contacts.
        //  normal = Vector3.UnitY;
        //  if (moB.Shape is HeightField)
        //    normal = -normal;
        //}
        //else
        //{
        //  // Use random normal.
        //  normal = RandomHelper.NextVector3(-1, 1).Normalized;
        //}

        // Check if the objects move towards or away from each other in the direction of the normal.
        if (normal != Vector3.Zero && Vector3.Dot(moB.LinearVelocity - moA.LinearVelocity, normal) <= 0)
        {
          // Objects move towards each other. --> Reflect their velocities.
          moA.LinearVelocity -= 2 * Vector3.ProjectTo(moA.LinearVelocity, normal);
          moB.LinearVelocity -= 2 * Vector3.ProjectTo(moB.LinearVelocity, normal);
          moA.AngularVelocity = -moA.AngularVelocity;
          moB.AngularVelocity = -moB.AngularVelocity;
        }
      }

      // Get the size of the current time step.
      float timeStep = (float)gameTime.ElapsedGameTime.TotalSeconds;

      // Move objects.
      var objects = _domain.CollisionObjects.Select(co => co.GeometricObject).OfType<MovingGeometricObject>();
      foreach (var obj in objects)
      {
        // Update position.
        Vector3 position = obj.Pose.Position + obj.LinearVelocity * timeStep;

        // Update rotation.
        Vector3 rotationAxis = obj.AngularVelocity;
        float angularSpeed = obj.AngularVelocity.Length;
        Matrix rotation = (Numeric.IsZero(angularSpeed))
          ? Matrix.Identity
          : Matrix.CreateRotation(rotationAxis, angularSpeed * timeStep);
        var orientation = rotation * obj.Pose.Orientation;

        // Incrementally updating the rotation matrix will eventually create a 
        // matrix which is not a rotation matrix anymore because of numerical 
        // problems. Re-othogonalization make sure that the matrix represents a
        // rotation.
        orientation.Orthogonalize();

        obj.Pose = new Pose(position, orientation);
      }




      // Update collision domain. This computes new contact information.      
      _domain.Update(timeStep);


      MessageBox.Show("Finished");
      Exit();


      // Record some statistics.
      int numberOfObjects = _domain.CollisionObjects.Count;
      Profiler.SetFormat("NumObjects", 1, "The total number of objects.");
      Profiler.AddValue("NumObjects", numberOfObjects);
      // If there are n objects, we can have max. n * (n - 1) / 2 collisions.
      Profiler.SetFormat("NumObjectPairs", 1, "The number of objects pairs, which have to be checked.");
      Profiler.AddValue("NumObjectPairs", numberOfObjects * (numberOfObjects - 1f) / 2f);
      // The first part of the collision detection is the "broad-phase" which
      // filters out objects that cannot collide (e.g. using a fast bounding box test).
      Profiler.SetFormat("BroadPhasePairs", 1, "The number of overlaps reported by the broad phase.");
      Profiler.AddValue("BroadPhasePairs", _domain.NumberOfBroadPhaseOverlaps);
      // Finally, the collision detection computes the exact contact information and creates
      // a ContactSet with the Contacts for each pair of colliding objects.
      Profiler.SetFormat("ContactSetCount", 1, "The number of actual collisions.");
      Profiler.AddValue("ContactSetCount", _domain.ContactSets.Count);

      // Draw objects using the DebugRenderer of the graphics screen.
      var debugRenderer = GraphicsScreen.DebugRenderer;
      debugRenderer.Clear();
      foreach (var collisionObject in _domain.CollisionObjects)
        debugRenderer.DrawObject(collisionObject.GeometricObject, GraphicsHelper.GetUniqueColor(collisionObject), false, false);
    }
  }
Ejemplo n.º 14
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            Debug.Assert(contactSet.Count <= 1, "Ray vs. plane should have at max 1 contact.");

              // Object A should be the plane.
              // Object B should be the ray.
              IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
              IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;

              // Swap objects if necessary.
              bool swapped = (rayObject.Shape is PlaneShape);
              if (swapped)
            MathHelper.Swap(ref planeObject, ref rayObject);

              PlaneShape planeShape = planeObject.Shape as PlaneShape;
              RayShape rayShape = rayObject.Shape as RayShape;

              // Check if A is really a plane and B is a ray.
              if (planeShape == null || rayShape == null)
            throw new ArgumentException("The contact set must contain a plane and a ray.", "contactSet");

              // Get transformations.
              Vector3F planeScale = planeObject.Scale;
              Vector3F rayScale = rayObject.Scale;
              Pose rayPose = rayObject.Pose;
              Pose planePose = planeObject.Pose;

              // Apply scale to plane.
              Plane plane = new Plane(planeShape);
              plane.Scale(ref planeScale);

              // Apply scale to ray and transform ray into local space of plane.
              Ray ray = new Ray(rayShape);
              ray.Scale(ref rayScale);      // Scale ray.
              ray.ToWorld(ref rayPose);     // Transform ray to world space.
              ray.ToLocal(ref planePose);   // Transform ray to local space of plane.

              // Convert ray into a line segment.
              LineSegment segment = new LineSegment { Start = ray.Origin, End = ray.Origin + ray.Direction * ray.Length };

              // Check if ray origin is inside the plane. Otherwise call plane vs. ray query.
              Vector3F linePoint;
              Vector3F planePoint = Vector3F.Zero;
              if (Vector3F.Dot(segment.Start, plane.Normal) <= plane.DistanceFromOrigin)
              {
            // The origin of the ray is below the plane.
            linePoint = segment.Start;
            contactSet.HaveContact = true;
              }
              else
              {
            // The origin of the ray is above the plane.
            contactSet.HaveContact = GeometryHelper.GetClosestPoints(plane, segment, out linePoint, out planePoint);
              }

              if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
              {
            // HaveContact queries can exit here.
            // GetContacts queries can exit here if we don't have a contact.
            return;
              }

              // ----- Create contact info.
              Vector3F position;
              float penetrationDepth;
              if (contactSet.HaveContact)
              {
            // We have a contact.
            position = planePose.ToWorldPosition(linePoint);
            penetrationDepth = (linePoint - segment.Start).Length;
              }
              else
              {
            // Closest points, but separated.
            position = planePose.ToWorldPosition((planePoint + linePoint) / 2);
            penetrationDepth = -(linePoint - planePoint).Length;
              }

              Vector3F normal = planePose.ToWorldDirection(plane.Normal);
              if (swapped)
            normal = -normal;

              Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, contactSet.HaveContact);
              ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
        }
Ejemplo n.º 15
0
    // OnUpdate() is called once per frame.
    protected override void OnUpdate(TimeSpan deltaTime)
    {
      if (_spring != null 
          && !_inputService.IsDown(MouseButtons.Left) 
          && !_inputService.IsDown(Buttons.LeftTrigger, LogicalPlayerIndex.One))
      {
        // The user has released the object.
        _simulation.Constraints.Remove(_spring);
        _spring = null;
      }

      if (!_inputService.IsMouseOrTouchHandled 
          && !_inputService.IsGamePadHandled(LogicalPlayerIndex.Any)
          && (_inputService.IsPressed(MouseButtons.Left, false) 
              || _inputService.IsPressed(Buttons.LeftTrigger, false, LogicalPlayerIndex.One)))
      {
        // The user has pressed the grab button and the input was not already handled
        // by another game object.

        // Remove the old joint, in case anything is grabbed.
        if (_spring != null)
        {
          _simulation.Constraints.Remove(_spring);
          _spring = null;
        }

        // The spring is attached at the position that is targeted with the cross-hair.
        // We can perform a ray hit-test to find the position. The ray starts at the camera
        // position and shoots forward (-z direction).
        var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
        var cameraNode = cameraGameObject.CameraNode;
        Vector3F cameraPosition = cameraNode.PoseWorld.Position;
        Vector3F cameraDirection = cameraNode.PoseWorld.ToWorldDirection(Vector3F.Forward);

        // Create a ray for picking.
        RayShape ray = new RayShape(cameraPosition, cameraDirection, 1000);

        // The ray should stop at the first hit. We only want the first object.
        ray.StopsAtFirstHit = true;

        // The collision detection requires a CollisionObject.
        CollisionObject rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));

        // Assign the collision object to collision group 2. (In SampleGame.cs a
        // collision filter based on collision groups was set. Objects for hit-testing
        // are in group 2.)
        rayCollisionObject.CollisionGroup = 2;

        // Get the first object that has contact with the ray.
        ContactSet contactSet = _simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
        if (contactSet != null && contactSet.Count > 0)
        {
          // The ray has hit something.

          // The contact set contains all detected contacts between the ray and the rigid body.
          // Get the first contact in the contact set. (A ray hit usually contains exactly 1 contact.)
          Contact contact = contactSet[0];

          // The contact set contains the object pair of the collision. One object is the ray.
          // The other is the object we want to grab.
          CollisionObject hitCollisionObject = (contactSet.ObjectA == rayCollisionObject) ? contactSet.ObjectB : contactSet.ObjectA;

          // Check whether a dynamic rigid body was hit.
          RigidBody hitBody = hitCollisionObject.GeometricObject as RigidBody;
          if (hitBody != null && hitBody.MotionType == MotionType.Dynamic)
          {
            // Attach the rigid body at the cursor position using a ball-socket joint.
            // (Note: We could also use a FixedJoint, if we don't want any rotations.)

            // The penetration depth tells us the distance from the ray origin to the rigid body.
            _springAttachmentDistanceFromObserver = contact.PenetrationDepth;

            // Get the position where the ray hits the other object.
            // (The position is defined in the local space of the object.)
            Vector3F hitPositionLocal = (contactSet.ObjectA == rayCollisionObject) ? contact.PositionBLocal : contact.PositionALocal;

            _spring = new BallJoint
            {
              BodyA = hitBody,
              AnchorPositionALocal = hitPositionLocal,

              // We need to attach the grabbed object to a second body. In this case we just want to
              // anchor the object at a specific point in the world. To achieve this we can use the
              // special rigid body "World", which is defined in the simulation.
              BodyB = _simulation.World,
              // AnchorPositionBLocal is set below.

              // Some constraint adjustments.
              ErrorReduction = 0.3f,

              // We set a softness > 0. This makes the joint "soft" and it will act like
              // damped spring. 
              Softness = 0.00001f,

              // We limit the maximal force. This reduces the ability of this joint to violate
              // other constraints. 
              MaxForce = 1e6f
            };

            // Add the spring to the simulation.
            _simulation.Constraints.Add(_spring);
          }
        }
      }

      if (_spring != null)
      {
        // User has grabbed something.

        // Update the position of the object by updating the anchor position of
        // the ball-socket joint.
        var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
        var cameraNode = cameraGameObject.CameraNode;
        Vector3F cameraPosition = cameraNode.PoseWorld.Position;
        Vector3F cameraDirection = cameraNode.PoseWorld.ToWorldDirection(-Vector3F.UnitZ);

        _spring.AnchorPositionBLocal = cameraPosition + cameraDirection * _springAttachmentDistanceFromObserver;

        // Reduce the angular velocity by a certain factor. (This acts like a damping because we
        // do not want the object to rotate like crazy.)
        _spring.BodyA.AngularVelocity *= 0.9f;
      }
    }
Ejemplo n.º 16
0
    private static void UpdateWheelContactInfo(ConstraintWheel wheel, ContactSet contactSet)
    {
      if (contactSet != null && contactSet.HaveContact && contactSet.Count > 0)
      {
        // ----- Ray has contact.
        var contact = contactSet[0];

        wheel.HasGroundContact = true;
        wheel.GroundPosition = contact.Position;
        if (wheel.CollisionObject == contactSet.ObjectA)
        {
          wheel.GroundNormal = -contact.Normal;
          wheel.TouchedBody = contactSet.ObjectB.GeometricObject as RigidBody;
        }
        else
        {
          wheel.GroundNormal = contact.Normal;
          wheel.TouchedBody = contactSet.ObjectA.GeometricObject as RigidBody;
        }

        // If the ray is nearly parallel to the ground, then the contact is not
        // useful and we ignore it.
        Vector3F up = wheel.Vehicle.Chassis.Pose.Orientation.GetColumn(1);
        float normalDotUp = Vector3F.Dot(wheel.GroundNormal, up);
        if (Numeric.IsGreater(normalDotUp, 0))
        {
          wheel.Tag = 1; // Tag = 1 means that this wheel has a useful ground contact.

          float hitDistance = contact.PenetrationDepth;
          wheel.SuspensionLength = Math.Max(hitDistance - wheel.Radius, wheel.MinSuspensionLength);

          wheel.Constraint.BodyB = wheel.TouchedBody ?? wheel.Vehicle.Simulation.World;
          wheel.Constraint.Enabled = true;

          wheel.GroundRight = wheel.Vehicle.Chassis.Pose.ToWorldDirection(Matrix33F.CreateRotationY(wheel.SteeringAngle) * Vector3F.UnitX);
          wheel.GroundForward = Vector3F.Cross(wheel.GroundNormal, wheel.GroundRight).Normalized;
        }
      }
      else
      {
        // -----  The wheel is in the air.          
        wheel.Constraint.Enabled = false;
        wheel.Constraint.BodyB = wheel.Vehicle.Simulation.World;
        wheel.HasGroundContact = false;
        wheel.TouchedBody = null;
        wheel.SuspensionLength = wheel.SuspensionRestLength;
      }
    }
Ejemplo n.º 17
0
        /// <summary>
        /// Performs a collision query to update the contact information in the contact set.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        /// <param name="deltaTime">
        /// The time step size in seconds. (The elapsed simulation time since the contact set was
        /// updated the last time.)
        /// </param>
        /// <remarks>
        /// <para>
        /// This method updates contact information stored in the given contact set. This method is
        /// usually faster than <see cref="GetContacts"/> because the information in
        /// <paramref name="contactSet"/> is reused and updated.
        /// </para>
        /// <para>
        /// The life time counter of persistent contacts is increased.
        /// </para>
        /// </remarks>
        /// <exception cref="ArgumentNullException">
        /// <paramref name="contactSet"/> is <see langword="null"/>.
        /// </exception>
        public void UpdateContacts(ContactSet contactSet, float deltaTime)
        {
            if (contactSet == null)
            {
                throw new ArgumentNullException("contactSet");
            }

            // Remove separated contacts - the contact set could contain separated
            // closest points of a GetClosestPoints query.
            ContactHelper.RemoveSeparatedContacts(contactSet);

            // Update cached information.
            ContactHelper.UpdateContacts(contactSet, deltaTime, CollisionDetection.ContactPositionTolerance);

            // We do not compute the detailed contact information for triggers.
            bool ignoreContactInfo = (contactSet.ObjectA.Type == CollisionObjectType.Trigger ||
                                      contactSet.ObjectB.Type == CollisionObjectType.Trigger);

            // We have to make the full contact query if one object is a ray with StopsAtFirstHit because
            // we need the PenetrationDepth for sorting the hits.
            if (contactSet.ObjectA.IsRayThatStopsAtFirstHit || contactSet.ObjectB.IsRayThatStopsAtFirstHit)
            {
                ignoreContactInfo = false;
            }

            if (ignoreContactInfo)
            {
                // Check cached flag. If necessary, make only boolean check.
                if (contactSet.HaveContact == false || contactSet.Count == 0)
                {
                    ComputeCollision(contactSet, CollisionQueryType.Boolean);
                }
            }
            else
            {
                // Compute new contact info.
                ComputeCollision(contactSet, CollisionQueryType.Contacts);
            }

            if (contactSet.HaveContact == false)
            {
                // No contact: Remove old contacts if objects do not touch.
                foreach (var contact in contactSet)
                {
                    contact.Recycle();
                }

                contactSet.Clear();
            }
            else
            {
                // Reduce ray cast results to 1 contact.
                if (contactSet.ObjectA.IsRay || contactSet.ObjectB.IsRay)
                {
                    ContactHelper.ReduceRayHits(contactSet);
                }

                // We have contact: Call contact filter.
                if (CollisionDetection.ContactFilter != null)
                {
                    CollisionDetection.ContactFilter.Filter(contactSet);
                }
            }

            CheckResult(contactSet, false);
            contactSet.IsValid = true;
        }
Ejemplo n.º 18
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;
            }
              }
        }
Ejemplo n.º 19
0
        public static void Merge(ContactSet target, ContactSet newContacts, CollisionQueryType type, float contactPositionTolerance)
        {
            Debug.Assert(target != null);
              Debug.Assert(newContacts != null);
              Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");

              int numberOfNewContacts = newContacts.Count;
              for (int i = 0; i < numberOfNewContacts; i++)
            Merge(target, newContacts[i], type, contactPositionTolerance);

              newContacts.Clear();
        }
Ejemplo n.º 20
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();
     }
       }
 }
Ejemplo n.º 21
0
        /// <summary>
        /// Performs more collision tests while slightly rotating one collision object.
        /// </summary>
        /// <param name="collisionDetection">The collision detection.</param>
        /// <param name="contactSet">
        /// The contact set; must contain at least 1 <see cref="Contact"/>.
        /// </param>
        /// <param name="perturbB">
        /// if set to <see langword="true"/> collision object B will be rotated; otherwise collision 
        /// object A will be rotated.
        /// </param>
        /// <param name="testMethod">The test method that is called to compute contacts.</param>
        /// <remarks>
        /// This method rotates one object 3 times and calls contact computation for the new
        /// orientations. It is recommended to call this method only when the contact set has 1 new
        /// contact.
        /// </remarks>
        internal static void TestWithPerturbations(CollisionDetection collisionDetection, ContactSet contactSet, bool perturbB, Action<ContactSet> testMethod)
        {
            Debug.Assert(contactSet != null);
              Debug.Assert(contactSet.Count > 0 && contactSet.HaveContact || !contactSet.IsPerturbationTestAllowed);
              Debug.Assert(testMethod != null);

              // Make this test only if there is 1 contact.
              // If there are 0 contacts, we assume that the contact pair is separated.
              // If there are more than 3 contacts, then we already have a lot of contacts to work with, no
              // need to search for more.
              if (!contactSet.HaveContact || contactSet.Count == 0 || contactSet.Count >= 4 || !contactSet.IsPerturbationTestAllowed)
            return;

              // Get data of object that will be rotated.
              var collisionObject = (perturbB) ? contactSet.ObjectB : contactSet.ObjectA;
              var geometricObject = collisionObject.GeometricObject;
              var pose = geometricObject.Pose;

              // Get normal, pointing to the test object.
              var normal = contactSet[0].Normal;
              if (!perturbB)
            normal = -normal;

              var contactPosition = contactSet[0].Position;
              var centerToContact = contactPosition - pose.Position;

              // Compute a perturbation angle proportional to the dimension of the object.
              var radius = geometricObject.Aabb.Extent.Length;
              var angle = collisionDetection.ContactPositionTolerance / radius;

              // axis1 is in the contact tangent plane, orthogonal to normal.
              var axis1 = Vector3F.Cross(normal, centerToContact);

              // If axis1 is zero then normal and centerToContact are collinear. This happens
              // for example for spheres or cone tips against flat faces. In these cases we assume
              // that there will be max. 1 contact.
              if (axis1.IsNumericallyZero)
            return;

              var axis1Local = pose.ToLocalDirection(axis1);
              var rotation = Matrix33F.CreateRotation(axis1Local, -angle);

              // Use temporary test objects.
              var testGeometricObject = TestGeometricObject.Create();
              testGeometricObject.Shape = geometricObject.Shape;
              testGeometricObject.Scale = geometricObject.Scale;
              testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation);

              var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
              testCollisionObject.SetInternal(collisionObject, testGeometricObject);

              var testContactSet = perturbB ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
                                    : ContactSet.Create(testCollisionObject, contactSet.ObjectB);
              testContactSet.IsPerturbationTestAllowed = false;             // Avoid recursive perturbation tests!
              testContactSet.PreferredNormal = contactSet.PreferredNormal;

              // Compute next contacts.
              testMethod(testContactSet);

              if (testContactSet.Count > 0)
              {
            // axis2 is in the contact tangent plane, orthogonal to axis1.
            var axis2 = Vector3F.Cross(axis1, normal);
            var axis2Local = pose.ToLocalDirection(axis2);

            var rotation2 = Matrix33F.CreateRotation(axis2Local, -angle);
            testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);

            // Compute next contacts.
            testMethod(testContactSet);

            // Invert rotation2.
            rotation2.Transpose();
            testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);

            // Compute next contacts.
            testMethod(testContactSet);
              }

              // Set HaveContact. It is reset when a perturbation separates the objects.
              testContactSet.HaveContact = true;

              // TODO: Test if we need this:
              // The contact world positions are not really correct because one object was rotated.
              // UpdateContacts recomputes the world positions from the local positions.
              UpdateContacts(testContactSet, 0, collisionDetection.ContactPositionTolerance);

              // Merge contacts of testContactSet into contact set, but do not change existing contacts.
              foreach (var contact in testContactSet)
              {
            // We call TryMerge() to see if the contact is similar to an existing contact.
            bool exists = TryMergeWithNearestContact(
              contactSet,
              contact,
              collisionDetection.ContactPositionTolerance,
              false);   // The existing contact must no be changed!

            if (exists)
            {
              // We can throw away the new contact because a similar is already in the contact set.
              contact.Recycle();
            }
            else
            {
              // Add new contact.
              contactSet.Add(contact);
            }
              }

              // Recycle temporary objects.
              testContactSet.Recycle();
              ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
              testGeometricObject.Recycle();
        }
Ejemplo n.º 22
0
        /// <summary>
        /// Reduces the number of contacts in the contact set to 1 contact.
        /// </summary>
        /// <param name="contactSet">
        /// The contact set. One shape in the contact set must be a <see cref="RayShape"/>!
        /// </param>
        /// <remarks>
        /// The best ray hit is kept. 
        /// </remarks>
        internal static void ReduceRayHits(ContactSet contactSet)
        {
            Debug.Assert(
            contactSet.ObjectA.GeometricObject.Shape is RayShape
            || contactSet.ObjectB.GeometricObject.Shape is RayShape,
            "ReduceRayHits was called for a contact set without a ray.");

              // For separated contacts keep the contact with the smallest separation.
              // If we have contact, keep the contact with the SMALLEST penetration (= shortest ray length)
              // and remove all invalid contacts (with separation).
              bool haveContact = contactSet.HaveContact;
              float bestPenetrationDepth = haveContact ? float.PositiveInfinity : float.NegativeInfinity;
              Contact bestContact = null;
              int numberOfContacts = contactSet.Count;
              for (int i = 0; i < numberOfContacts; i++)
              {
            Contact contact = contactSet[i];
            float penetrationDepth = contact.PenetrationDepth;

            if (haveContact)
            {
              // Search for positive and smallest penetration depth.
              if (penetrationDepth >= 0 && penetrationDepth < bestPenetrationDepth)
              {
            bestContact = contact;
            bestPenetrationDepth = penetrationDepth;
              }
            }
            else
            {
              // Search for negative and largest penetration depth (Separation!)
              Debug.Assert(penetrationDepth < 0, "HaveContact is false, but contact shows penetration.");
              if (penetrationDepth > bestPenetrationDepth)
              {
            bestContact = contact;
            bestPenetrationDepth = penetrationDepth;
              }
            }
              }

              // Keep best contact.
              // Note: In some situations HaveContact is true, but the contact set does not contains any
              // contacts with penetration. This happen, for example, when testing a ray inside a triangle
              // mesh. The TriangleMeshAlgorithm automatically filters contacts with bad normals.
              // When HaveContact is false, we should always have a contact (closest point).

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

              contactSet.Clear();
              if (bestContact != null)
            contactSet.Add(bestContact);
        }
Ejemplo n.º 23
0
        /// <summary>
        /// Reduces the number of contacts in the contact set to 1 contact.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        /// <remarks>
        /// The contact with the biggest penetration depth is kept.
        /// </remarks>
        internal static void ReduceClosestPoints(ContactSet contactSet)
        {
            // Reduce to 1 contact.
              int numberOfContacts = contactSet.Count;
              if (numberOfContacts > 1)
              {
            // Keep the contact with the deepest penetration depth.
            Contact bestContact = contactSet[0];
            for (int i = 1; i < numberOfContacts; i++)
            {
              if (contactSet[i].PenetrationDepth > bestContact.PenetrationDepth)
              {
            bestContact = contactSet[i];
              }
            }

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

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

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

              // If we HaveContact but the contact shows a separation, delete contact.
              // This can happen for TriangleMesh vs. TriangleMesh because the triangle mesh algorithm
              // filters contacts if they have a bad normal. It can happen that all contacts are filtered
              // and only a separated contact remains in the contact set.
              if (contactSet.HaveContact && contactSet.Count > 0 && contactSet[0].PenetrationDepth < 0)
              {
            contactSet[0].Recycle();
            contactSet.Clear();
              }
        }
Ejemplo n.º 24
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();
            }
              }
        }
Ejemplo n.º 25
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 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);
            }
        }
Ejemplo n.º 26
0
        /// <summary>
        /// Tries to the merge the contact with the nearest contact in the given 
        /// <see cref="ContactSet"/>.
        /// </summary>
        /// <param name="contactSet">The contact set. (Must not be <see langword="null"/>.)</param>
        /// <param name="contact">The contact. (Must not be <see langword="null"/>.)</param>
        /// <param name="contactPositionTolerance">The contact position tolerance.</param>
        /// <param name="updateMerged">
        /// If set to <see langword="true"/> the merged contact is updated with the data of 
        /// <paramref name="contact"/>. If set to <see langword="false"/> the merged contact keeps the
        /// data of the old contact.
        /// </param>
        /// <returns>
        /// <see langword="true"/> if the contact was merged successfully; otherwise
        /// <see langword="false"/>.
        /// </returns>
        private static bool TryMergeWithNearestContact(ContactSet contactSet, Contact contact, float contactPositionTolerance, bool updateMerged)
        {
            Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
              Debug.Assert(contactSet.Count > 0, "Cannot merge nearest contact to empty contact set.");

              // Find the cached contact that is closest.
              int nearestContactIndex = -1;

              // Near contact must be within contact position tolerance.
              float minDistance = contactPositionTolerance + Numeric.EpsilonF;
              float minDistanceSquared = minDistance * minDistance;

              int numberOfContacts = contactSet.Count;
              for (int i = 0; i < numberOfContacts; i++)
              {
            Contact otherContact = contactSet[i];

            // Do not merge contacts with different features because user should be notified by
            // a new contact that a new feature is touched.
            if (contact.FeatureA == otherContact.FeatureA && contact.FeatureB == otherContact.FeatureB)
            {
              // Check position difference.
              float distanceSquared = (otherContact.Position - contact.Position).LengthSquared;
              if (distanceSquared < minDistanceSquared)
              {
            minDistanceSquared = distanceSquared;
            nearestContactIndex = i;
              }
            }
              }

              if (nearestContactIndex >= 0)
              {
            if (updateMerged)
            {
              // Merge with an existing contact.
              // We take the geometry of the new contact and keep the other data of the old contact.
              // We also keep the old contact, so that references to this contact stay valid.
              Contact nearestContact = contactSet[nearestContactIndex];
              nearestContact.IsRayHit = contact.IsRayHit;
              nearestContact.PositionALocal = contact.PositionALocal;
              nearestContact.PositionBLocal = contact.PositionBLocal;
              nearestContact.Normal = contact.Normal;
              nearestContact.PenetrationDepth = contact.PenetrationDepth;
              nearestContact.Position = contact.Position;
            }

            return true;
              }

              return false;
        }
Ejemplo n.º 27
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            // Object A should be the plane.
            // Object B should be the sphere.
            IGeometricObject planeObject  = contactSet.ObjectA.GeometricObject;
            IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject;

            // A should be the plane, swap objects if necessary.
            bool swapped = (sphereObject.Shape is PlaneShape);

            if (swapped)
            {
                MathHelper.Swap(ref planeObject, ref sphereObject);
            }

            PlaneShape  planeShape  = planeObject.Shape as PlaneShape;
            SphereShape sphereShape = sphereObject.Shape as SphereShape;

            // Check if collision object shapes are correct.
            if (planeShape == null || sphereShape == null)
            {
                throw new ArgumentException("The contact set must contain a plane and a sphere.", "contactSet");
            }

            // Get scalings.
            Vector3F planeScale  = planeObject.Scale;
            Vector3F sphereScale = Vector3F.Absolute(sphereObject.Scale);

            // Call other algorithm for non-uniformly scaled spheres.
            if (sphereScale.X != sphereScale.Y || sphereScale.Y != sphereScale.Z)
            {
                if (_fallbackAlgorithm == null)
                {
                    _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(PlaneShape), typeof(ConvexShape)];
                }

                _fallbackAlgorithm.ComputeCollision(contactSet, type);
                return;
            }

            // Get poses.
            Pose planePose  = planeObject.Pose;
            Pose spherePose = sphereObject.Pose;

            // Apply scaling to plane and transform plane to world space.
            Plane plane = new Plane(planeShape);

            plane.Scale(ref planeScale);     // Scale plane.
            plane.ToWorld(ref planePose);    // Transform plane to world space.

            // Calculate distance from plane to sphere surface.
            float    sphereRadius          = sphereShape.Radius * sphereScale.X;
            Vector3F sphereCenter          = spherePose.Position;
            float    planeToSphereDistance = Vector3F.Dot(sphereCenter, plane.Normal) - sphereRadius - plane.DistanceFromOrigin;

            float penetrationDepth = -planeToSphereDistance;

            contactSet.HaveContact = (penetrationDepth >= 0);

            if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
            {
                // HaveContact queries can exit here.
                // GetContacts queries can exit here if we don't have a contact.
                return;
            }

            // Compute contact details.
            Vector3F position = sphereCenter - plane.Normal * (sphereRadius - penetrationDepth / 2);
            Vector3F normal   = (swapped) ? -plane.Normal : plane.Normal;

            // Update contact set.
            Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);

            ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
        }
Ejemplo n.º 28
0
        /// <summary>
        /// Updates the contact geometry for a single contact.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        /// <param name="contact">The contact to be updated.</param>
        /// <param name="contactPositionTolerance">The contact position tolerance.</param>
        /// <returns>
        /// <see langword="true"/> if the contact is invalid and should be removed.
        /// </returns>
        private static bool UpdateContact(ContactSet contactSet, Contact contact, float contactPositionTolerance)
        {
            Pose poseA = contactSet.ObjectA.GeometricObject.Pose;
              Pose poseB = contactSet.ObjectB.GeometricObject.Pose;

              // Get local positions in world space.
              //Vector3F positionA = poseA.ToWorldPosition(contact.PositionALocal);
              //Vector3F positionB = poseB.ToWorldPosition(contact.PositionBLocal);

              // ----- Optimized version:
              Vector3F positionALocal = contact.PositionALocal;
              Vector3F positionA;
              positionA.X = poseA.Orientation.M00 * positionALocal.X + poseA.Orientation.M01 * positionALocal.Y + poseA.Orientation.M02 * positionALocal.Z + poseA.Position.X;
              positionA.Y = poseA.Orientation.M10 * positionALocal.X + poseA.Orientation.M11 * positionALocal.Y + poseA.Orientation.M12 * positionALocal.Z + poseA.Position.Y;
              positionA.Z = poseA.Orientation.M20 * positionALocal.X + poseA.Orientation.M21 * positionALocal.Y + poseA.Orientation.M22 * positionALocal.Z + poseA.Position.Z;
              Vector3F positionBLocal = contact.PositionBLocal;
              Vector3F positionB;
              positionB.X = poseB.Orientation.M00 * positionBLocal.X + poseB.Orientation.M01 * positionBLocal.Y + poseB.Orientation.M02 * positionBLocal.Z + poseB.Position.X;
              positionB.Y = poseB.Orientation.M10 * positionBLocal.X + poseB.Orientation.M11 * positionBLocal.Y + poseB.Orientation.M12 * positionBLocal.Z + poseB.Position.Y;
              positionB.Z = poseB.Orientation.M20 * positionBLocal.X + poseB.Orientation.M21 * positionBLocal.Y + poseB.Orientation.M22 * positionBLocal.Z + poseB.Position.Z;

              // Update Position.
              contact.Position = (positionA + positionB) / 2;

              // Update contacts and closest points differently:
              if (contact.PenetrationDepth >= 0)
              {
            // ----- Contact.
            Vector3F bToA = positionA - positionB;  // Vector from contact on A to contact on B
            if (!contact.IsRayHit)
            {
              // ----- Normal contact.
              // Update penetration depth: Difference of world position projected onto normal.
              //contact.PenetrationDepth = Vector3F.Dot(bToA, contact.Normal);

              // ----- Optimized version:
              Vector3F contactNormal = contact.Normal;
              contact.PenetrationDepth = bToA.X * contactNormal.X + bToA.Y * contactNormal.Y + bToA.Z * contactNormal.Z;
            }
            else
            {
              // ----- Ray hit.
              // Update penetration depth: Contact position to ray origin projected onto ray direction.

              // Get ray. Only one shape is a ray because ray vs. ray do normally not collide.
              RayShape ray = contactSet.ObjectA.GeometricObject.Shape as RayShape;
              float rayScale = contactSet.ObjectA.GeometricObject.Scale.X;   // Non-uniformly scaled rays are not support, so we only need Scale.X!
              Vector3F hitPositionLocal;  // Hit position in local space of ray.
              if (ray != null)
              {
            hitPositionLocal = poseA.ToLocalPosition(contact.Position);
              }
              else
              {
            // The other object must be the ray.
            ray = contactSet.ObjectB.GeometricObject.Shape as RayShape;
            rayScale = contactSet.ObjectB.GeometricObject.Scale.X;   // Non-uniformly scaled rays are not support, so we only need Scale.X!
            hitPositionLocal = poseB.ToLocalPosition(contact.Position);
              }

              // Now, we have found the ray, unless there is a composite shape with a child ray - which
              // is not supported.
              if (ray != null)
              {
            contact.PenetrationDepth = Vector3F.Dot(hitPositionLocal - ray.Origin * rayScale, ray.Direction);

            // If the new penetration depth is negative or greater than the ray length,
            // the objects have separated along the ray direction.
            if (contact.PenetrationDepth < 0 || contact.PenetrationDepth > ray.Length * rayScale)
              return true;
              }
            }

            // Remove points with negative penetration depth.
            if (contact.PenetrationDepth < 0)
              return true;

            // Check drift.
            float driftSquared;
            if (contact.IsRayHit)
            {
              // For ray casts: Remove contact if movement in any direction is too large.
              driftSquared = bToA.LengthSquared;
            }
            else
            {
              // For contacts: Remove contacts if horizontal movement (perpendicular to contact normal)
              // is too large.
              driftSquared = (bToA - contact.Normal * contact.PenetrationDepth).LengthSquared;
            }

            // Remove contact if drift is too large.
            return driftSquared > contactPositionTolerance * contactPositionTolerance;
              }
              else
              {
            // ----- Closest point pair.

            // Update distance. Since we do not check the geometric objects, the new distance
            // could be a separation or a penetration. We assume it is a separation and
            // use a "-" sign.
            // We have no problem if we are wrong and this is actually a penetration because this
            // contact is automatically updated or removed when new contacts are computed in
            // the narrow phase.
            Vector3F aToB = positionB - positionA;  // Vector from contact on A to contact on B
            contact.PenetrationDepth = -aToB.Length;

            // If points moved into contact, remove this pair, because we don't have a valid
            // contact normal.
            if (Numeric.IsZero(contact.PenetrationDepth))
              return true;

            // Update normal.
            contact.Normal = aToB.Normalized;

            return false;
              }
        }
Ejemplo n.º 29
0
      /// <summary>
      /// Filters the specified contact set.
      /// </summary>
      /// <param name="contactSet">The contact set.</param>
      public void Filter(ContactSet contactSet)
      {
        // Call the default contact filter.
        DefaultContactFilter.Filter(contactSet);

        // Abort if there are no contacts in this contact set.
        if (contactSet.Count == 0)
          return;

        // If this is a sphere vs. * contact set, then we correct the position of the 
        // contact point to make sure that the contact position is in line with the sphere center.

        var sphere = contactSet.ObjectA.GeometricObject.Shape as SphereShape;
        if (sphere != null)
        {
          float radius = sphere.Radius;

          foreach (var contact in contactSet)
            contact.Position = contactSet.ObjectA.GeometricObject.Pose.Position + contact.Normal * (radius - contact.PenetrationDepth / 2);

          return;
        }

        sphere = contactSet.ObjectB.GeometricObject.Shape as SphereShape;
        if (sphere != null)
        {
          float radius = sphere.Radius;

          foreach (var contact in contactSet)
            contact.Position = contactSet.ObjectB.GeometricObject.Pose.Position - contact.Normal * (radius - contact.PenetrationDepth / 2);
        }
      }
Ejemplo n.º 30
0
        public static Contact CreateContact(ContactSet contactSet,
            Vector3F position,
            Vector3F normal,
            float penetrationDepth,
            bool isRayHit)
        {
            Debug.Assert(contactSet != null);

              return CreateContact(contactSet.ObjectA, contactSet.ObjectB, position, normal, penetrationDepth, isRayHit);
        }
Ejemplo n.º 31
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            if (type == CollisionQueryType.Contacts)
            {
                throw new GeometryException("GJK cannot handle contact queries. Use MPR instead.");
            }

            IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
            ConvexShape      shapeA  = objectA.Shape as ConvexShape;
            Vector3F         scaleA  = objectA.Scale;
            Pose             poseA   = objectA.Pose;

            IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
            ConvexShape      shapeB  = objectB.Shape as ConvexShape;
            Vector3F         scaleB  = objectB.Scale;
            Pose             poseB   = objectB.Pose;

            if (shapeA == null || shapeB == null)
            {
                throw new ArgumentException("The contact set must contain two convex shapes.", "contactSet");
            }

            // GJK builds a simplex of the CSO (A-B). This simplex is managed in a GjkSimplexSolver.
            var  simplex             = GjkSimplexSolver.Create();
            bool foundSeparatingAxis = false;

            try
            {
                // v is the separating axis or the CSO point nearest to the origin.
                // We start with last known separating axis or with an arbitrary CSO point.
                Vector3F v;
                if (contactSet.Count > 0)
                {
                    // Use last separating axis.
                    // The contact normal points from A to B. This is the direction we want to sample first.
                    // If the frame-to-frame coherence is high we should get a point close to the origin.
                    // Note: To sample in the normal direction, we need to initialize the CSO point v with
                    // -normal.
                    v = -contactSet[0].Normal;
                }
                else
                {
                    // Use difference of inner points.
                    Vector3F vA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
                    Vector3F vB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
                    v = vA - vB;
                }

                // If the inner points overlap, then we have already found a contact.
                // We don't expect this case to happen often, so we simply choose an arbitrary separating
                // axis to continue with the normal GJK code.
                if (v.IsNumericallyZero)
                {
                    v = Vector3F.UnitZ;
                }

                // Cache inverted rotations.
                var orientationAInverse = poseA.Orientation.Transposed;
                var orientationBInverse = poseB.Orientation.Transposed;

                int   iterationCount  = 0;
                float distanceSquared = float.MaxValue;
                float distanceEpsilon;

                // Assume we have no contact.
                contactSet.HaveContact = false;

                do
                {
                    // TODO: Translate A and B close to the origin to avoid numerical problems.
                    // This optimization is done in Bullet: The offset (a.Pose.Position + b.Pose.Position) / 2
                    // is subtracted from a.Pose and b.Pose. This offset is added when the Contact info is
                    // computed (also in EPA if the poses are still translated).

                    // Compute a new point w on the simplex. We seek for the point that is closest to the origin.
                    // Therefore, we get the support points on the current separating axis v.
                    Vector3F p = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -v, scaleA));
                    Vector3F q = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * v, scaleB));
                    Vector3F w = p - q;

                    // w projected onto the separating axis.
                    float delta = Vector3F.Dot(w, v);

                    // If v∙w > 0 then the objects do not overlap.
                    if (delta > 0)
                    {
                        // We have found a separating axis.
                        foundSeparatingAxis = true;

                        // Early exit for boolean and contact queries.
                        if (type == CollisionQueryType.Boolean || type == CollisionQueryType.Contacts)
                        {
                            // TODO: We could cache the separating axis n in ContactSet for future collision checks.
                            return;
                        }

                        // We continue for closest point queries because we don't know if there are other
                        // points closer than p and q.
                    }

                    // If the new w is already part of the simplex. We cannot improve any further.
                    if (simplex.Contains(w))
                    {
                        break;
                    }

                    // If the new w is not closer to the origin (within numerical tolerance), we stop.
                    if (distanceSquared - delta <= distanceSquared * Numeric.EpsilonF) // SOLID uses Epsilon = 10^-6
                    {
                        break;
                    }

                    // Add the new point to the simplex.
                    simplex.Add(w, p, q);

                    // Update the simplex. (Unneeded simplex points are removed).
                    simplex.Update();

                    // Get new point of simplex closest to the origin.
                    v = simplex.ClosestPoint;

                    float previousDistanceSquared = distanceSquared;
                    distanceSquared = v.LengthSquared;

                    if (previousDistanceSquared < distanceSquared)
                    {
                        // If the result got worse, we use the previous result. This happens for
                        // degenerate cases for example when the simplex is a tetrahedron with all
                        // 4 vertices in a plane.
                        distanceSquared = previousDistanceSquared;
                        simplex.Backup();
                        break;
                    }

                    // If the new simplex is invalid, we stop.
                    // Example: A simplex gets invalid if a fourth vertex is added to create a tetrahedron
                    // simplex but all vertices are in a plane. This can happen if a box corner nearly touches a
                    // face of another box.
                    if (!simplex.IsValid)
                    {
                        break;
                    }

                    // Compare the distance of v to the origin with the distance of the last iteration.
                    // We stop if the improvement is less than the numerical tolerance.
                    if (previousDistanceSquared - distanceSquared <= previousDistanceSquared * Numeric.EpsilonF)
                    {
                        break;
                    }

                    // If we reach the iteration limit, we stop.
                    iterationCount++;
                    if (iterationCount > MaxNumberOfIterations)
                    {
                        Debug.Assert(false, "GJK reached the iteration limit.");
                        break;
                    }

                    // Compute a scaled epsilon.
                    distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared);

                    // Loop until the simplex is full (i.e. it contains the origin) or we have come
                    // sufficiently close to the origin.
                } while (!simplex.IsFull && distanceSquared > distanceEpsilon);

                Debug.Assert(simplex.IsEmpty == false, "The GJK simplex must contain at least 1 point.");

                // Compute contact normal and separation.
                Vector3F normal = -simplex.ClosestPoint; // simplex.ClosestPoint = ClosestPointA-ClosestPointB
                float    distance;
                distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared);
                if (distanceSquared <= distanceEpsilon)
                {
                    // Distance is approximately 0.
                    // --> Objects are in contact.
                    if (simplex.IsValid && normal.TryNormalize())
                    {
                        // Normal can be used but we have to invert it because for contact we
                        // have to compute normal as pointOnA - pointOnB.
                        normal = -normal;
                    }
                    else
                    {
                        // No useful normal. Use direction between inner points as a fallback.
                        Vector3F innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
                        normal = simplex.ClosestPointOnA - innerA;
                        if (!normal.TryNormalize())
                        {
                            Vector3F innerB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
                            normal = innerB - innerA;
                            if (!normal.TryNormalize())
                            {
                                normal = Vector3F.UnitY;
                                // TODO: We could use better normal: e.g. normal of old contact or PreferredNormal?
                            }
                        }
                    }

                    distance = 0;
                    contactSet.HaveContact = true;
                }
                else
                {
                    // Distance is greater than 0.
                    distance = (float)Math.Sqrt(distanceSquared);
                    normal  /= distance;

                    // If the simplex is valid and full, then we have a contact.
                    if (simplex.IsFull && simplex.IsValid)
                    {
                        // Let's use the current result as an estimated contact info for
                        // shallow contacts.

                        // TODO: The following IF was added because this can occur for valid
                        // triangle vs. triangle separation. Check this.
                        if (!foundSeparatingAxis)
                        {
                            contactSet.HaveContact = true;
                            // Distance is a penetration depth
                            distance = -distance;

                            // Since the simplex tetrahedron can have any position in the Minkowsky difference,
                            // we do not know the real normal. Let's use the current normal and make
                            // sure that it points away from A. - This is only a heuristic...
                            Vector3F innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
                            if (Vector3F.Dot(simplex.ClosestPointOnA - innerA, normal) < 0)
                            {
                                normal = -normal;
                            }
                        }
                    }
                }

                Debug.Assert(normal.IsNumericallyZero == false);

                if (type != CollisionQueryType.Boolean)
                {
                    Vector3F position = (simplex.ClosestPointOnA + simplex.ClosestPointOnB) / 2;
                    Contact  contact  = ContactHelper.CreateContact(contactSet, position, normal, -distance, false);
                    ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
                }
            }
            finally
            {
                simplex.Recycle();
            }
        }
Ejemplo n.º 32
0
    public override void Update(GameTime gameTime)
    {
      var debugRenderer = GraphicsScreen.DebugRenderer;
      debugRenderer.Clear();

      // Draw coordinate cross at world origin.
      debugRenderer.DrawAxes(Pose.Identity, 10, false);

      // Draw objects.
      debugRenderer.DrawObject(_objectA.GeometricObject, GraphicsHelper.GetUniqueColor(_objectA), _drawWireframe, false);
      debugRenderer.DrawObject(_objectB.GeometricObject, GraphicsHelper.GetUniqueColor(_objectB), _drawWireframe, false);

      // Draw contact info.
      debugRenderer.DrawContacts(_contactSet, 0.1f, null, true);

      // Toggle wireframe rendering.
      if (InputService.IsPressed(Keys.Space, true))
        _drawWireframe = !_drawWireframe;

      // Change background color if we have a contact.
      GraphicsScreen.BackgroundColor = (_contactSet != null && _contactSet.HaveContact)
        ? new Color(220, 200, 200, 255)
        : new Color(200, 220, 200, 255);

      // Move one object with keyboard NumPad.
      var translation = new Vector3F();
      if (InputService.IsDown(Keys.NumPad4))
        translation.X -= 1;
      if (InputService.IsDown(Keys.NumPad6))
        translation.X += 1;
      if (InputService.IsDown(Keys.NumPad8))
        translation.Y += 1;
      if (InputService.IsDown(Keys.NumPad5))
        translation.Y -= 1;
      if (InputService.IsDown(Keys.NumPad7))
        translation.Z -= 1;
      if (InputService.IsDown(Keys.NumPad9))
        translation.Z += 1;

      if (!translation.IsNumericallyZero)
      {
        var go = (GeometricObject)_objectA.GeometricObject;

        float scale = go.Aabb.Extent.Length * 0.1f;
        translation *= scale * (float)gameTime.ElapsedGameTime.TotalSeconds;
        
        go.Pose = new Pose(go.Pose.Position + translation, go.Pose.Orientation);

        if (_contactSet != null)
          _collisionDetection.UpdateContacts(_contactSet, 0.001f);
        else
          _contactSet = _collisionDetection.GetContacts(_objectA, _objectB);
      }
    }
Ejemplo n.º 33
0
 /// <summary>
 /// Computes the collision. - This method should only be used by
 /// <see cref="CollisionAlgorithm"/> instances!
 /// </summary>
 /// <param name="contactSet">The contact set.</param>
 /// <param name="type">The type of collision query.</param>
 /// <remarks>
 /// <para>
 /// This method does the real work. It is called from the other public methods of a
 /// <see cref="CollisionAlgorithm"/>. Also, if one <see cref="CollisionAlgorithm"/> uses another
 /// <see cref="CollisionAlgorithm"/> internally, this method should be called directly instead
 /// of <see cref="CollisionAlgorithm.UpdateClosestPoints"/> or
 /// <see cref="CollisionAlgorithm.UpdateContacts"/>.
 /// </para>
 /// <para>
 /// <strong>Notes to Inheritors:</strong> This is the central method which has to be implemented
 /// in derived classes. <paramref name="contactSet"/> is never <see langword="null"/>. This
 /// method has to add new contact/closest-point info with
 /// <see cref="ContactHelper.Merge(ContactSet,Contact,CollisionQueryType,float)"/>. It is not
 /// necessary to remove old contacts. At the beginning of the method
 /// <see cref="ContactSet.HaveContact"/> in <paramref name="contactSet"/> indicates the result
 /// of the last narrow phase algorithm that was run on <paramref name="contactSet"/>. This
 /// method must set <see cref="ContactSet.HaveContact"/> to <see langword="false"/> if it
 /// doesn't find a contact or to <see langword="true"/> if it finds a contact.
 /// </para>
 /// </remarks>
 public abstract void ComputeCollision(ContactSet contactSet, CollisionQueryType type);
Ejemplo n.º 34
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            if (type == CollisionQueryType.ClosestPoints)
              {
            // Just use normal composite shape algorithm.
            _triangleMeshAlgorithm.ComputeCollision(contactSet, type);
            return;
              }

              Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");

              // Mesh = A, Ray = B
              IGeometricObject meshObject = contactSet.ObjectA.GeometricObject;
              IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;

              // Object A should be the mesh, swap objects if necessary.
              bool swapped = (meshObject.Shape is RayShape);
              if (swapped)
            MathHelper.Swap(ref rayObject, ref meshObject);

              RayShape rayShape = rayObject.Shape as RayShape;
              TriangleMeshShape meshShape = meshObject.Shape as TriangleMeshShape;

              // Check if shapes are correct.
              if (rayShape == null || meshShape == null)
            throw new ArgumentException("The contact set must contain a ray and a triangle mesh shape.", "contactSet");

              // Assume no contact.
              contactSet.HaveContact = false;

              // Get transformations.
              Vector3F rayScale = rayObject.Scale;
              Pose rayPose = rayObject.Pose;
              Vector3F meshScale = meshObject.Scale;
              Pose meshPose = meshObject.Pose;

              // Ray in world space.
              Ray rayWorld = new Ray(rayShape);
              rayWorld.Scale(ref rayScale);     // Scale ray.
              rayWorld.ToWorld(ref rayPose);    // Transform ray to world space.

              // Ray in local scaled space of the mesh.
              Ray ray = rayWorld;
              ray.ToLocal(ref meshPose);   // Transform ray to local space of composite.

              // Ray in local unscaled space of the mesh.
              Ray rayUnscaled = ray;
              var inverseCompositeScale = Vector3F.One / meshScale;
              rayUnscaled.Scale(ref inverseCompositeScale);

              ITriangleMesh triangleMesh = meshShape.Mesh;
              bool isTwoSided = meshShape.IsTwoSided;

              if (meshShape.Partition != null)
              {
            // ----- Mesh with BVH vs. Ray -----
            foreach (var childIndex in meshShape.Partition.GetOverlaps(rayUnscaled))
            {
              Triangle triangle = triangleMesh.GetTriangle(childIndex);

              AddContact(contactSet, swapped, type, ref rayWorld, ref ray, ref triangle, childIndex, ref meshPose, ref meshScale, isTwoSided);

              if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
            break; // We can abort early.
            }
              }
              else
              {
            // ----- Mesh vs. Ray -----
            var rayUnscaledDirectionInverse = new Vector3F(
            1 / rayUnscaled.Direction.X,
            1 / rayUnscaled.Direction.Y,
            1 / rayUnscaled.Direction.Z);

            float epsilon = Numeric.EpsilonF * (1 + meshObject.Aabb.Extent.Length);

            int numberOfTriangles = triangleMesh.NumberOfTriangles;
            for (int i = 0; i < numberOfTriangles; i++)
            {
              Triangle triangle = triangleMesh.GetTriangle(i);

              // Make ray vs AABB check first. We could skip this because the ray vs. triangle test
              // is also fast. But experiments (ray vs sphere mesh) have shown that making an
              // additional ray vs. AABB test first makes the worst case more than 20% faster.
              if (GeometryHelper.HaveContact(triangle.Aabb, rayUnscaled.Origin, rayUnscaledDirectionInverse, rayUnscaled.Length, epsilon))
              {
            AddContact(contactSet, swapped, type, ref rayWorld, ref ray, ref triangle, i, ref meshPose, ref meshScale, isTwoSided);

            // We have contact and stop for boolean queries.
            if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
              break;
              }
            }
              }
        }
Ejemplo n.º 35
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            // Ray vs. convex has at max 1 contact.
            Debug.Assert(contactSet.Count <= 1);

            // Object A should be the ray.
            // Object B should be the convex.
            IGeometricObject rayObject    = contactSet.ObjectA.GeometricObject;
            IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;

            // Swap object if necessary.
            bool swapped = (convexObject.Shape is RayShape);

            if (swapped)
            {
                MathHelper.Swap(ref rayObject, ref convexObject);
            }

            RayShape    rayShape    = rayObject.Shape as RayShape;
            ConvexShape convexShape = convexObject.Shape as ConvexShape;

            // Check if shapes are correct.
            if (rayShape == null || convexShape == null)
            {
                throw new ArgumentException("The contact set must contain a ray and a convex shape.", "contactSet");
            }

            // Call line segment vs. convex for closest points queries.
            if (type == CollisionQueryType.ClosestPoints)
            {
                // Find point on ray closest to the convex shape.

                // Call GJK.
                _gjk.ComputeCollision(contactSet, type);
                if (contactSet.HaveContact == false)
                {
                    return;
                }

                // Otherwise compute 1 contact ...
                // GJK result is invalid for penetration.
                foreach (var contact in contactSet)
                {
                    contact.Recycle();
                }

                contactSet.Clear();
            }

            // Assume no contact.
            contactSet.HaveContact = false;

            // Get transformations.
            Vector3 rayScale    = rayObject.Scale;
            Vector3 convexScale = convexObject.Scale;
            Pose    convexPose  = convexObject.Pose;
            Pose    rayPose     = rayObject.Pose;

            // See Raycasting paper of van den Bergen or Bullet.
            // Note: Compute in local space of convex (object B).

            // Scale ray and transform ray to local space of convex.
            Ray rayWorld = new Ray(rayShape);

            rayWorld.Scale(ref rayScale);  // Scale ray.
            rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
            Ray ray = rayWorld;

            ray.ToLocal(ref convexPose); // Transform ray to local space of convex.

            var simplex = GjkSimplexSolver.Create();

            try
            {
                Vector3 s = ray.Origin;                 // source
                Vector3 r = ray.Direction * ray.Length; // ray
                float   λ = 0;                          // ray parameter
                Vector3 x = s;                          // hit spot (on ray)
                Vector3 n = new Vector3();              // normal
                Vector3 v = x - convexShape.GetSupportPoint(ray.Direction, convexScale);
                // v = x - arbitrary point. Vector used for support mapping.
                float distanceSquared = v.LengthSquared(); // ||v||²
                int   iterationCount  = 0;

                while (distanceSquared > Numeric.EpsilonF && iterationCount < MaxNumberOfIterations)
                {
                    iterationCount++;
                    Vector3 p = convexShape.GetSupportPoint(v, convexScale); // point on convex
                    Vector3 w = x - p;                                       // simplex/Minkowski difference point

                    float vDotW = Vector3.Dot(v, w);                         // v∙w
                    if (vDotW > 0)
                    {
                        float vDotR = Vector3.Dot(v, r); // v∙r
                        if (vDotR >= 0)                  // TODO: vDotR >= - Epsilon^2 ?
                        {
                            return;                      // No Hit.
                        }
                        λ = λ - vDotW / vDotR;
                        x = s + λ * r;
                        simplex.Clear(); // Configuration space obstacle (CSO) is translated whenever x is updated.
                        w = x - p;
                        n = v;
                    }

                    simplex.Add(w, x, p);
                    simplex.Update();
                    v = simplex.ClosestPoint;
                    distanceSquared = (simplex.IsValid && !simplex.IsFull) ? v.LengthSquared() : 0;
                }

                // We have a contact if the hit is inside the ray length.
                contactSet.HaveContact = (0 <= λ && λ <= 1);

                if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
                {
                    // HaveContact queries can exit here.
                    // GetContacts queries can exit here if we don't have a contact.
                    return;
                }

                float penetrationDepth = λ * ray.Length;

                Debug.Assert(contactSet.HaveContact, "Separation was not detected by GJK above.");

                // Convert back to world space.
                Vector3 position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
                n = convexPose.ToWorldDirection(n);
                if (!n.TryNormalize())
                {
                    n = Vector3.UnitY;
                }

                if (swapped)
                {
                    n = -n;
                }

                // Update contact set.
                Contact contact = ContactHelper.CreateContact(contactSet, position, -n, penetrationDepth, true);
                ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
            }
            finally
            {
                simplex.Recycle();
            }
        }
Ejemplo n.º 36
0
        private void ComputeLineVsOther(ContactSet contactSet, CollisionQueryType type, bool objectAIsLine)
        {
            CollisionObject  collisionObjectA = contactSet.ObjectA;
            CollisionObject  collisionObjectB = contactSet.ObjectB;
            IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
            IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
            Shape            shapeA           = geometricObjectA.Shape;
            Shape            shapeB           = geometricObjectB.Shape;

            Debug.Assert(
                shapeA is LineShape && !(shapeB is LineShape) ||
                shapeB is LineShape && !(shapeA is LineShape),
                "LineAlgorithm.ComputeLineVsOther should only be called for a line and another shape.");

            CollisionObject  lineCollisionObject;
            IGeometricObject lineGeometricObject;
            IGeometricObject otherGeometricObject;
            LineShape        lineShape;
            Shape            otherShape;

            if (objectAIsLine)
            {
                lineCollisionObject  = collisionObjectA;
                lineGeometricObject  = geometricObjectA;
                lineShape            = (LineShape)shapeA;
                otherGeometricObject = geometricObjectB;
                otherShape           = shapeB;
            }
            else
            {
                lineCollisionObject  = collisionObjectB;
                lineGeometricObject  = geometricObjectB;
                lineShape            = (LineShape)shapeB;
                otherGeometricObject = geometricObjectA;
                otherShape           = shapeA;
            }

            // Apply scaling to line.
            Line     line      = new Line(lineShape);
            Vector3F lineScale = lineGeometricObject.Scale;

            line.Scale(ref lineScale);

            // Step 1: Get any bounding sphere that encloses the other object.
            Aabb     aabb   = otherGeometricObject.Aabb;
            Vector3F center = (aabb.Minimum + aabb.Maximum) / 2;
            float    radius = (aabb.Maximum - aabb.Minimum).Length; // A large safe radius. (Exact size does not matter.)

            // Step 2: Get the closest point of line vs. center.
            // All computations in local space of the line.
            Vector3F closestPointOnLine;
            Pose     linePose = lineGeometricObject.Pose;

            GeometryHelper.GetClosestPoint(line, linePose.ToLocalPosition(center), out closestPointOnLine);

            // Step 3: Crop the line to a line segment that will contain the closest point.
            var lineSegment = ResourcePools.LineSegmentShapes.Obtain();

            lineSegment.Start = closestPointOnLine - line.Direction * radius;
            lineSegment.End   = closestPointOnLine + line.Direction * radius;

            // Use temporary test objects.
            var testGeometricObject = TestGeometricObject.Create();

            testGeometricObject.Shape = lineSegment;
            testGeometricObject.Scale = Vector3F.One;
            testGeometricObject.Pose  = linePose;

            var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();

            testCollisionObject.SetInternal(lineCollisionObject, testGeometricObject);

            var testContactSet = objectAIsLine ? ContactSet.Create(testCollisionObject, collisionObjectB)
                                         : ContactSet.Create(collisionObjectA, testCollisionObject);

            testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;

            // Step 4: Call another collision algorithm.
            CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[lineSegment, otherShape];

            // Step 5: Manually chosen preferred direction for MPR.
            // For the MPR we choose the best ray direction ourselves. The ray should be normal
            // to the line, otherwise MPR could try to push the line segment out of the other object
            // in the line direction - this cannot work for infinite lines.
            // Results without a manual MPR ray were ok for normal cases. Problems were only observed
            // for cases where the InnerPoints overlap or for deep interpenetrations.
            Vector3F v0A = geometricObjectA.Pose.ToWorldPosition(shapeA.InnerPoint * geometricObjectA.Scale);
            Vector3F v0B = geometricObjectB.Pose.ToWorldPosition(shapeB.InnerPoint * geometricObjectB.Scale);
            Vector3F n   = v0B - v0A; // This is the default MPR ray direction.

            // Make n normal to the line.
            n = n - Vector3F.ProjectTo(n, linePose.ToWorldDirection(lineShape.Direction));
            if (!n.TryNormalize())
            {
                n = lineShape.Direction.Orthonormal1;
            }

            testContactSet.PreferredNormal = n;
            collisionAlgorithm.ComputeCollision(testContactSet, type);

            if (testContactSet.HaveContact)
            {
                contactSet.HaveContact = true;
            }

            ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);

            // Recycle temporary objects.
            testContactSet.Recycle();
            ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
            testGeometricObject.Recycle();
            ResourcePools.LineSegmentShapes.Recycle(lineSegment);
        }
Ejemplo n.º 37
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            IGeometricObject sphereObjectA = contactSet.ObjectA.GeometricObject;
            IGeometricObject sphereObjectB = contactSet.ObjectB.GeometricObject;
            SphereShape      sphereShapeA  = sphereObjectA.Shape as SphereShape;
            SphereShape      sphereShapeB  = sphereObjectB.Shape as SphereShape;

            // Check if collision objects are spheres.
            if (sphereShapeA == null || sphereShapeB == null)
            {
                throw new ArgumentException("The contact set must contain sphere shapes.", "contactSet");
            }

            Vector3F scaleA = Vector3F.Absolute(sphereObjectA.Scale);
            Vector3F scaleB = Vector3F.Absolute(sphereObjectB.Scale);

            // Call MPR for non-uniformly scaled spheres.
            if (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z ||
                scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z)
            {
                if (_fallbackAlgorithm == null)
                {
                    _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), typeof(ConvexShape)];
                }

                _fallbackAlgorithm.ComputeCollision(contactSet, type);
                return;
            }

            // Apply uniform scale.
            float radiusA = sphereShapeA.Radius * scaleA.X;
            float radiusB = sphereShapeB.Radius * scaleB.X;

            // Vector from center of A to center of B.
            Vector3F centerA    = sphereObjectA.Pose.Position;
            Vector3F centerB    = sphereObjectB.Pose.Position;
            Vector3F aToB       = centerB - centerA;
            float    lengthAToB = aToB.Length;

            // Check radius of spheres.
            float penetrationDepth = radiusA + radiusB - lengthAToB;

            contactSet.HaveContact = penetrationDepth >= 0;

            if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
            {
                // HaveContact queries can exit here.
                // GetContacts queries can exit here if we don't have a contact.
                return;
            }

            // ----- Create contact information.
            Vector3F normal;

            if (Numeric.IsZero(lengthAToB))
            {
                // Spheres are on the same position, we can choose any normal vector.
                // Possibly it would be better to consider the object movement (velocities), but
                // it is not important since this case should be VERY rare.
                normal = Vector3F.UnitY;
            }
            else
            {
                normal = aToB.Normalized;
            }

            // The contact point lies in the middle of the intersecting volume.
            Vector3F position = centerA + normal * (radiusA - penetrationDepth / 2);

            // Update contact set.
            Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);

            ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
        }
Ejemplo n.º 38
0
        // See FAST paper: "Interactive Continuous Collision Detection for Non-Convex Polyhedra", Kim Young et al.

        /// <summary>
        /// Gets the time of impact using Conservative Advancement (ignoring rotational movement).
        /// </summary>
        /// <param name="objectA">The object A.</param>
        /// <param name="targetPoseA">The target pose of A.</param>
        /// <param name="objectB">The object B.</param>
        /// <param name="targetPoseB">The target pose of B.</param>
        /// <param name="allowedPenetration">The allowed penetration depth.</param>
        /// <param name="collisionDetection">The collision detection.</param>
        /// <returns>
        /// The time of impact in the range [0, 1].
        /// </returns>
        /// <remarks>
        /// This algorithm does not work for concave objects.
        /// </remarks>
        /// <exception cref="ArgumentNullException">
        /// <paramref name="objectA"/>, <paramref name="objectB"/> or
        /// <paramref name="collisionDetection"/> is <see langword="null"/>.
        /// </exception>
        internal static float GetTimeOfImpactLinearCA(CollisionObject objectA, Pose targetPoseA,
                                                      CollisionObject objectB, Pose targetPoseB, float allowedPenetration,
                                                      CollisionDetection collisionDetection) // Required for collision algorithm matrix.
        {
            if (objectA == null)
            {
                throw new ArgumentNullException("objectA");
            }
            if (objectB == null)
            {
                throw new ArgumentNullException("objectB");
            }
            if (collisionDetection == null)
            {
                throw new ArgumentNullException("collisionDetection");
            }

            IGeometricObject geometricObjectA = objectA.GeometricObject;
            IGeometricObject geometricObjectB = objectB.GeometricObject;

            Pose startPoseA = geometricObjectA.Pose;
            Pose startPoseB = geometricObjectB.Pose;

            // Compute relative linear velocity.
            // (linearRelVel ∙ normal > 0 if objects are getting closer.)
            Vector3F linearVelocityA        = targetPoseA.Position - startPoseA.Position;
            Vector3F linearVelocityB        = targetPoseB.Position - startPoseB.Position;
            Vector3F linearVelocityRelative = linearVelocityA - linearVelocityB;

            // Abort if relative movement is zero.
            if (Numeric.IsZero(linearVelocityRelative.Length))
            {
                return(1);
            }

            var distanceAlgorithm = collisionDetection.AlgorithmMatrix[objectA, objectB];

            // Use temporary test objects.
            var testGeometricObjectA = TestGeometricObject.Create();

            testGeometricObjectA.Shape = geometricObjectA.Shape;
            testGeometricObjectA.Scale = geometricObjectA.Scale;
            testGeometricObjectA.Pose  = startPoseA;

            var testGeometricObjectB = TestGeometricObject.Create();

            testGeometricObjectB.Shape = geometricObjectB.Shape;
            testGeometricObjectB.Scale = geometricObjectB.Scale;
            testGeometricObjectB.Pose  = startPoseB;

            var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();

            testCollisionObjectA.SetInternal(objectA, testGeometricObjectA);

            var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();

            testCollisionObjectB.SetInternal(objectB, testGeometricObjectB);

            var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);

            try
            {
                distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);

                if (testContactSet.Count < 0)
                {
                    // No closest-distance result. --> Abort.
                    return(1);
                }

                Vector3F normal   = testContactSet[0].Normal;
                float    distance = -testContactSet[0].PenetrationDepth;

                float λ         = 0;
                float λPrevious = 0;

                for (int i = 0; i < MaxNumberOfIterations && distance > 0; i++)
                {
                    // |v∙n|
                    float velocityProjected = Vector3F.Dot(linearVelocityRelative, normal);

                    // Abort for separating objects.
                    if (Numeric.IsLess(velocityProjected, 0))
                    {
                        break;
                    }

                    // Increase TOI.
                    float μ = (distance + allowedPenetration) / velocityProjected;
                    λ = λ + μ;

                    if (λ < 0 || λ > 1)
                    {
                        break;
                    }

                    Debug.Assert(λPrevious < λ);

                    if (λ <= λPrevious)
                    {
                        break;
                    }

                    // Get new interpolated poses - only positions are changed.
                    Vector3F positionA = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position);
                    testGeometricObjectA.Pose = new Pose(positionA, startPoseA.Orientation);

                    Vector3F positionB = startPoseB.Position + λ * (targetPoseB.Position - startPoseB.Position);
                    testGeometricObjectB.Pose = new Pose(positionB, startPoseB.Orientation);

                    // Get new closest point distance.
                    distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
                    if (testContactSet.Count == 0)
                    {
                        break;
                    }

                    normal   = testContactSet[0].Normal;
                    distance = -testContactSet[0].PenetrationDepth;

                    λPrevious = λ;
                }

                if (testContactSet.HaveContact && λ > 0 && λ < 1 && testContactSet.Count > 0)
                {
                    return(λ);
                    // We already have a contact that we could use.
                    // result.Contact = testContactSet[0];
                }
            }
            finally
            {
                // Recycle temporary objects.
                testContactSet.Recycle(true);
                ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
                ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
                testGeometricObjectA.Recycle();
                testGeometricObjectB.Recycle();
            }

            return(1);
        }
Ejemplo n.º 39
0
 public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
 {
     contactSet.HaveContact = true;
 }
Ejemplo n.º 40
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            // Note: When comparing this implementation with the GJK (see Gjk.cs), be aware that the
            // GJK implementation computes the CSO as the Minkowski difference A-B whereas the MPR uses
            // B-A. Both representations of the CSO are equivalent, we just have to invert the vectors
            // here and there. (B-A was chosen because the original description of the MPR used B-A.)

            if (type == CollisionQueryType.ClosestPoints)
            {
                throw new GeometryException("MPR cannot handle closest-point queries. Use GJK instead.");
            }

            CollisionObject  collisionObjectA = contactSet.ObjectA;
            IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
            ConvexShape      shapeA           = geometricObjectA.Shape as ConvexShape;
            Vector3          scaleA           = geometricObjectA.Scale;
            Pose             poseA            = geometricObjectA.Pose;

            CollisionObject  collisionObjectB = contactSet.ObjectB;
            IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
            ConvexShape      shapeB           = geometricObjectB.Shape as ConvexShape;
            Vector3          scaleB           = geometricObjectB.Scale;
            Pose             poseB            = geometricObjectB.Pose;

            if (shapeA == null || shapeB == null)
            {
                throw new ArgumentException("The contact set must contain convex shapes.", "contactSet");
            }

            // Assume no contact.
            contactSet.HaveContact = false;

            Vector3 v0;

            if (contactSet.IsPreferredNormalAvailable && type == CollisionQueryType.Contacts)
            {
                // Set v0, so to shoot into preferred direction.
                v0 = contactSet.PreferredNormal;

                // Perform only 1 MPR iteration.
                DoMpr(type, contactSet, v0);
                return;
            }

            // Find first point v0 (which determines the ray direction).
            // Inner point in CSO (Minkowski difference B-A).
            Vector3 v0A = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
            Vector3 v0B = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);

            v0 = v0B - v0A;

            // If v0 == origin then we have contact.
            if (v0.IsNumericallyZero)
            {
                // The inner points overlap. Probably there are two objects centered on the same point.
                contactSet.HaveContact = true;
                if (type == CollisionQueryType.Boolean)
                {
                    return;
                }

                // Choose a v0 different from Zero. Any direction is ok.
                // The point should still be in the Minkowski difference.
                v0.X = CollisionDetection.Epsilon / 10;
            }

            // Call MPR in iteration until the MPR ray has converged.
            int       iterationCount = 0;
            const int iterationLimit = 10;
            Vector3   oldMprRay;

            // Use a temporary contact set.
            var testContactSet = ContactSet.Create(collisionObjectA, collisionObjectB);

            testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;
            testContactSet.PreferredNormal           = contactSet.PreferredNormal;

            Contact oldContact = null;

            do
            {
                oldMprRay = v0;
                if (iterationCount == 0)
                {
                    oldMprRay.TryNormalize();
                }

                // Call MPR. v0 of the next iteration is simply -returned portal normal.

                Debug.Assert(testContactSet.Count == 0 || testContactSet.Count == 1, "testContactSet in MPR should have 0 or 1 contacts.");
                Debug.Assert(testContactSet.Count == 0 || testContactSet[0] == oldContact);
                testContactSet.Clear();

                // Because of numerical problems (for example with long thin ellipse vs. capsule)
                // it is possible that the last iteration was a contact but in this iteration
                // no contact is found. Therefore we also reset the HaveContact flag to avoid
                // an end result where HaveContact is set but no Contact is in the ContactSet.
                testContactSet.HaveContact = false;
                v0 = -DoMpr(type, testContactSet, v0);

                if (testContactSet.Count > 0)
                {
                    var newContact = testContactSet[0];
                    if (oldContact != null)
                    {
                        if (oldContact.PenetrationDepth < newContact.PenetrationDepth)
                        {
                            // The new penetration depth is larger then the old penetration depth.
                            // In this case we keep the old contact.
                            // This can happen for nearly parallel boxes. First we get a good contact.
                            // Then we get a contact another side. Normal has changed 90�. The new
                            // penetration depth can be nearly the whole box side length :-(.
                            newContact.Recycle();
                            testContactSet[0] = oldContact;
                            break;
                        }
                    }

                    if (newContact != oldContact)
                    {
                        if (oldContact != null)
                        {
                            oldContact.Recycle();
                        }

                        oldContact = newContact;
                    }
                }

                iterationCount++;
            } while (testContactSet.HaveContact && // Separation? - No contact which we could refine.
                     iterationCount < iterationLimit && // Iteration limit reached?
                     v0 != Vector3.Zero &&         // Is normal useful to go on?
                     !Vector3.AreNumericallyEqual(-v0, oldMprRay, CollisionDetection.Epsilon));
            // Normal hasn't converged yet?

            if (testContactSet.Count > 0)
            {
                // Recycle oldContact if not used.
                if (testContactSet[0] != oldContact)
                {
                    if (oldContact != null)
                    {
                        oldContact.Recycle();
                        oldContact = null;
                    }
                }
            }

            if (CollisionDetection.FullContactSetPerFrame &&
                type == CollisionQueryType.Contacts &&
                testContactSet.Count > 0 &&
                contactSet.Count < 3)
            {
                // Try to find full contact set.
                var wrapper = TestMethodWrappers.Obtain();
                wrapper.OriginalMethod = _doMprMethod;
                wrapper.V0             = testContactSet[0].Normal; // The MPR ray will point along the normal of the first contact.

                ContactHelper.TestWithPerturbations(
                    CollisionDetection,
                    testContactSet,
                    true,
                    wrapper.Method);

                TestMethodWrappers.Recycle(wrapper);
            }

            contactSet.HaveContact = testContactSet.HaveContact;
            ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);

            // Recycle temporary objects.
            testContactSet.Recycle();
        }
        // 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
        }
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            if (type == CollisionQueryType.ClosestPoints)
            {
                // Just use normal height field shape algorithm.
                _heightFieldAlgorithm.ComputeCollision(contactSet, type);
                return;
            }

            Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");

            // HeightField = A, Ray = B
            IGeometricObject heightFieldObject = contactSet.ObjectA.GeometricObject;
            IGeometricObject rayObject         = contactSet.ObjectB.GeometricObject;

            // Object A should be the height field, swap objects if necessary.
            bool swapped = (heightFieldObject.Shape is RayShape);

            if (swapped)
            {
                MathHelper.Swap(ref rayObject, ref heightFieldObject);
            }

            RayShape    rayShape    = rayObject.Shape as RayShape;
            HeightField heightField = heightFieldObject.Shape as HeightField;

            // Check if shapes are correct.
            if (rayShape == null || heightField == null)
            {
                throw new ArgumentException("The contact set must contain a ray and a height field.", "contactSet");
            }

            // Assume no contact.
            contactSet.HaveContact = false;

            // Get transformations.
            Vector3F rayScale         = rayObject.Scale;
            Pose     rayPose          = rayObject.Pose;
            Vector3F heightFieldScale = heightFieldObject.Scale;
            Pose     heightFieldPose  = heightFieldObject.Pose;

            // We do not support negative scaling. It is not clear what should happen when y is
            // scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
            if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0)
            {
                throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
            }

            // Ray in world space.
            Ray rayWorld = new Ray(rayShape);

            rayWorld.Scale(ref rayScale);
            rayWorld.ToWorld(ref rayPose);

            // Ray in local scaled space of the height field.
            Ray rayScaled = rayWorld;

            rayScaled.ToLocal(ref heightFieldPose);

            // Ray in local unscaled space of the mesh.
            Ray rayUnscaled           = rayScaled;
            var inverseCompositeScale = Vector3F.One / heightFieldScale;

            rayUnscaled.Scale(ref inverseCompositeScale);

            // Get height field and basic info.
            int arrayLengthX   = heightField.NumberOfSamplesX;
            int arrayLengthZ   = heightField.NumberOfSamplesZ;
            int numberOfCellsX = arrayLengthX - 1;
            int numberOfCellsZ = arrayLengthZ - 1;

            Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
            float cellWidthX = heightField.WidthX / numberOfCellsX; // Unscaled!
            float cellWidthZ = heightField.WidthZ / numberOfCellsZ; // Unscaled!

            // We use a 2D-DDA traversal of the height field cells. In other words: Look at it from
            // above. The height field is our screen and we will select the cells as if we draw
            // a pixel line. This could be made more efficient when we do not recompute values and
            // reuse values and make incremental steps Bresenham-style.
            // See GeometryHelper_Casts.cs method HaveContact(Aabb, ray) for explanation of the
            // ray parameter formula.

            var rayUnscaledDirectionInverse = new Vector3F(
                1 / rayUnscaled.Direction.X,
                1 / rayUnscaled.Direction.Y,
                1 / rayUnscaled.Direction.Z);

            // The position where the ray enters the current cell.
            var cellEnter = rayUnscaled.Origin; // Unscaled!!!

            var originX = heightField.OriginX;
            var originZ = heightField.OriginZ;

            // ----- Find first cell.
            int indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; // (int)(...) does not return the desired result for negative values!

            if (indexX < 0)
            {
                if (rayUnscaled.Direction.X <= 0)
                {
                    return;
                }

                float parameter = (originX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
                if (parameter > rayUnscaled.Length)
                {
                    return; // The ray does not reach the height field.
                }
                cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
                indexX    = 0;
            }
            else if (indexX >= numberOfCellsX)
            {
                if (rayUnscaled.Direction.X >= 0)
                {
                    return;
                }

                float parameter = (originX + heightField.WidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
                if (parameter > rayUnscaled.Length)
                {
                    return; // The ray does not reach the height field.
                }
                cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
                indexX    = numberOfCellsX - 1;
            }

            int indexZ = (cellEnter.Z >= originZ) ? (int)((cellEnter.Z - originZ) / cellWidthZ) : -1;

            if (indexZ < 0)
            {
                if (rayUnscaled.Direction.Z <= 0)
                {
                    return;
                }

                float parameter = (originZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
                if (parameter > rayUnscaled.Length)
                {
                    return; // The ray does not reach the next height field.
                }
                cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
                // We also have to correct the indexX!
                indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
                indexZ = 0;
            }
            else if (indexZ >= numberOfCellsZ)
            {
                if (rayUnscaled.Direction.Z >= 0)
                {
                    return;
                }

                float parameter = (originZ + heightField.WidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
                if (parameter > rayUnscaled.Length)
                {
                    return; // The ray does not reach the next height field.
                }
                cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
                indexX    = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
                indexZ    = numberOfCellsZ - 1;
            }

            if (indexX < 0 || indexX >= numberOfCellsX || indexZ < 0 || indexZ >= numberOfCellsZ)
            {
                return;
            }

            while (true)
            {
                // ----- Get triangles of current cell.
                var triangle0 = heightField.GetTriangle(indexX, indexZ, false);
                var triangle1 = heightField.GetTriangle(indexX, indexZ, true);

                // Index of first triangle.
                var triangleIndex = (indexZ * numberOfCellsX + indexX) * 2;

                float xRelative           = (cellEnter.X - originX) / cellWidthX - indexX;
                float zRelative           = (cellEnter.Z - originZ) / cellWidthZ - indexZ;
                bool  enterSecondTriangle = (xRelative + zRelative) > 1; // The diagonal is where xRel + zRel == 1.

                // ----- Find cell exit and move indices to next cell.
                // The position where the ray leaves the current cell.
                Vector3F cellExit;
                float    nextXParameter = float.PositiveInfinity;
                if (rayUnscaled.Direction.X > 0)
                {
                    nextXParameter = (originX + (indexX + 1) * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
                }
                else if (rayUnscaled.Direction.X < 0)
                {
                    nextXParameter = (originX + indexX * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
                }

                float nextZParameter = float.PositiveInfinity;
                if (rayUnscaled.Direction.Z > 0)
                {
                    nextZParameter = (originZ + (indexZ + 1) * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
                }
                else if (rayUnscaled.Direction.Z < 0)
                {
                    nextZParameter = (originZ + indexZ * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
                }

                bool isLastCell = false;
                if (nextXParameter < nextZParameter)
                {
                    if (rayUnscaled.Direction.X > 0)
                    {
                        indexX++;
                        if (indexX >= numberOfCellsX) // Abort if we have left the height field.
                        {
                            isLastCell = true;
                        }
                    }
                    else
                    {
                        indexX--;
                        if (indexX < 0)
                        {
                            isLastCell = true;
                        }
                    }

                    if (nextXParameter > rayUnscaled.Length)
                    {
                        isLastCell     = true; // The ray does not reach the next cell.
                        nextXParameter = rayUnscaled.Length;
                    }

                    cellExit = rayUnscaled.Origin + nextXParameter * rayUnscaled.Direction;
                }
                else
                {
                    if (rayUnscaled.Direction.Z > 0)
                    {
                        indexZ++;
                        if (indexZ >= numberOfCellsZ)
                        {
                            isLastCell = true;
                        }
                    }
                    else
                    {
                        indexZ--;
                        if (indexZ < 0)
                        {
                            isLastCell = true;
                        }
                    }

                    if (nextZParameter > rayUnscaled.Length)
                    {
                        isLastCell     = true;
                        nextZParameter = rayUnscaled.Length;
                    }

                    cellExit = rayUnscaled.Origin + nextZParameter * rayUnscaled.Direction;
                }


                // ----- We can skip cell if cell AABB is below the ray.
                var rayMinY = Math.Min(cellEnter.Y, cellExit.Y) - CollisionDetection.Epsilon; // Apply to avoid missing collisions when ray hits a cell border.

                // The ray is above if no height field height is higher the ray height.
                // (This check handles NaN height values (holes) correctly.)
                bool rayIsAbove = !(triangle0.Vertex0.Y >= rayMinY ||
                                    triangle0.Vertex1.Y >= rayMinY ||
                                    triangle0.Vertex2.Y >= rayMinY ||
                                    triangle1.Vertex1.Y >= rayMinY); // Vertex1 of triangle1 is the fourth quad vertex!

                // ----- Test ray against the 2 triangles of the cell.
                bool triangle0IsHole = false;
                bool triangle1IsHole = false;
                if (!rayIsAbove)
                {
                    // Abort if a height value is NaN (hole).
                    triangle0IsHole = Numeric.IsNaN(triangle0.Vertex0.Y * triangle0.Vertex1.Y * triangle0.Vertex2.Y);
                    triangle1IsHole = Numeric.IsNaN(triangle1.Vertex0.Y * triangle1.Vertex1.Y * triangle1.Vertex2.Y);

                    bool contactAdded = false;
                    if (enterSecondTriangle)
                    {
                        // Test second triangle first.
                        if (!triangle1IsHole)
                        {
                            contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
                        }
                        if (!contactAdded && !triangle0IsHole)
                        {
                            contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
                        }
                    }
                    else
                    {
                        // Test first triangle first.
                        if (!triangle0IsHole)
                        {
                            contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
                        }
                        if (!contactAdded && !triangle1IsHole)
                        {
                            contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
                        }
                    }

                    if (contactAdded)
                    {
                        return;
                    }

                    // We have contact and stop for boolean queries.
                    if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
                    {
                        return;
                    }
                }

                // ----- Return simplified contact if cellEnter is below the cell.
                if (!rayIsAbove)
                {
                    if (!enterSecondTriangle && !triangle0IsHole && GeometryHelper.IsInFront(triangle0, cellEnter) < 0 ||
                        enterSecondTriangle && !triangle1IsHole && GeometryHelper.IsInFront(triangle1, cellEnter) < 0)
                    {
                        contactSet.HaveContact = true;

                        if (type == CollisionQueryType.Boolean)
                        {
                            return;
                        }

                        var position = heightFieldPose.ToWorldPosition(cellEnter * heightFieldScale);
                        var normal   = heightFieldPose.ToWorldDirection(Vector3F.UnitY);
                        if (swapped)
                        {
                            normal = -normal;
                        }

                        float   penetrationDepth = (position - rayWorld.Origin).Length;
                        Contact contact          = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
                        ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
                        return;
                    }
                }

                // ----- Move to next cell.
                if (isLastCell)
                {
                    return;
                }

                cellEnter = cellExit;
            }
        }
Ejemplo n.º 43
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            if (type == CollisionQueryType.ClosestPoints)
            {
                // Just use normal composite shape algorithm.
                _compositeAlgorithm.ComputeCollision(contactSet, type);
                return;
            }

            Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");

            // Composite = A, Ray = B
            IGeometricObject compositeObject = contactSet.ObjectA.GeometricObject;
            IGeometricObject rayObject       = contactSet.ObjectB.GeometricObject;

            // Object A should be the composite, swap objects if necessary.
            bool swapped = (compositeObject.Shape is RayShape);

            if (swapped)
            {
                MathHelper.Swap(ref rayObject, ref compositeObject);
            }

            RayShape       rayShape       = rayObject.Shape as RayShape;
            CompositeShape compositeShape = compositeObject.Shape as CompositeShape;

            // Check if shapes are correct.
            if (rayShape == null || compositeShape == null)
            {
                throw new ArgumentException("The contact set must contain a ray and a composite shape.", "contactSet");
            }

            // Assume no contact.
            contactSet.HaveContact = false;

            // Get transformations.
            Vector3F rayScale       = rayObject.Scale;
            Pose     rayPose        = rayObject.Pose;
            Vector3F compositeScale = compositeObject.Scale;
            Pose     compositePose  = compositeObject.Pose;

            // Check if transforms are supported.
            // Same check for object B.
            if (compositeShape != null &&
                (compositeScale.X != compositeScale.Y || compositeScale.Y != compositeScale.Z) &&
                compositeShape.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
            {
                throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
            }

            // ----- A few fixed objects which are reused to avoid GC garbage.
            var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
            var testGeometricObject = TestGeometricObject.Create();

            // Create a test contact set and initialize with dummy objects.
            // (The actual collision objects are set below.)
            var testContactSet = ContactSet.Create(testCollisionObject, contactSet.ObjectB); // Dummy arguments! They are changed later.

            // Scale ray and transform ray to local unscaled space of composite.
            Ray rayWorld = new Ray(rayShape);

            rayWorld.Scale(ref rayScale);  // Scale ray.
            rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
            Ray ray = rayWorld;

            ray.ToLocal(ref compositePose); // Transform ray to local space of composite.
            var inverseCompositeScale = Vector3F.One / compositeScale;

            ray.Scale(ref inverseCompositeScale);

            try
            {
                if (compositeShape.Partition != null)
                {
                    #region ----- Composite with BVH vs. * -----

                    foreach (var childIndex in compositeShape.Partition.GetOverlaps(ray))
                    {
                        if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
                        {
                            break; // We can abort early.
                        }
                        AddChildContacts(
                            contactSet,
                            swapped,
                            childIndex,
                            type,
                            testContactSet,
                            testCollisionObject,
                            testGeometricObject);
                    }
                    #endregion
                }
                else
                {
                    #region ----- Composite vs. *-----

                    var rayDirectionInverse = new Vector3F(
                        1 / ray.Direction.X,
                        1 / ray.Direction.Y,
                        1 / ray.Direction.Z);

                    float epsilon = Numeric.EpsilonF * (1 + compositeObject.Aabb.Extent.Length);

                    // Go through list of children and find contacts.
                    int numberOfChildGeometries = compositeShape.Children.Count;
                    for (int i = 0; i < numberOfChildGeometries; i++)
                    {
                        IGeometricObject child = compositeShape.Children[i];

                        if (GeometryHelper.HaveContact(child.Shape.GetAabb(child.Scale, child.Pose), ray.Origin, rayDirectionInverse, ray.Length, epsilon))
                        {
                            AddChildContacts(
                                contactSet,
                                swapped,
                                i,
                                type,
                                testContactSet,
                                testCollisionObject,
                                testGeometricObject);

                            // We have contact and stop for boolean queries.
                            if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
                            {
                                break;
                            }
                        }
                    }
                    #endregion
                }
            }
            finally
            {
                Debug.Assert(compositeObject.Shape == compositeShape, "Shape was altered and not restored.");

                testContactSet.Recycle();
                ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
                testGeometricObject.Recycle();
            }
        }
Ejemplo n.º 44
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            // Invoke GJK for closest points.
            if (type == CollisionQueryType.ClosestPoints)
            {
                throw new GeometryException("This collision algorithm cannot handle closest-point queries. Use GJK instead.");
            }

            CollisionObject  collisionObjectA = contactSet.ObjectA;
            CollisionObject  collisionObjectB = contactSet.ObjectB;
            IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
            IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
            BoxShape         boxA             = geometricObjectA.Shape as BoxShape;
            BoxShape         boxB             = geometricObjectB.Shape as BoxShape;

            // Check if collision objects shapes are correct.
            if (boxA == null || boxB == null)
            {
                throw new ArgumentException("The contact set must contain box shapes.", "contactSet");
            }

            Vector3F scaleA = Vector3F.Absolute(geometricObjectA.Scale);
            Vector3F scaleB = Vector3F.Absolute(geometricObjectB.Scale);
            Pose     poseA  = geometricObjectA.Pose;
            Pose     poseB  = geometricObjectB.Pose;

            // We perform the separating axis test in the local space of A.
            // The following variables are in local space of A.

            // Center of box B.
            Vector3F cB = poseA.ToLocalPosition(poseB.Position);
            // Orientation matrix of box B
            Matrix33F mB = poseA.Orientation.Transposed * poseB.Orientation;
            // Absolute of mB.
            Matrix33F aMB = Matrix33F.Absolute(mB);

            // Half extent vectors of the boxes.
            Vector3F eA = 0.5f * boxA.Extent * scaleA;
            Vector3F eB = 0.5f * boxB.Extent * scaleB;

            // ----- Separating Axis tests
            // If the boxes are separated, we immediately return.
            // For the case of interpenetration, we store the smallest penetration depth.
            float    smallestPenetrationDepth = float.PositiveInfinity;
            int      separatingAxisNumber     = 0;
            Vector3F normal           = Vector3F.UnitX;
            bool     isNormalInverted = false;

            contactSet.HaveContact = false;   // Assume no contact.

            #region ----- Case 1: Separating Axis: (1, 0, 0) -----
            float separation = Math.Abs(cB.X) - (eA.X + eB.X * aMB.M00 + eB.Y * aMB.M01 + eB.Z * aMB.M02);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
            {
                normal = Vector3F.UnitX;
                smallestPenetrationDepth = -separation;
                isNormalInverted         = cB.X < 0;
                separatingAxisNumber     = 1;
            }
            #endregion

            #region ----- Case 2: Separating Axis: (0, 1, 0) -----
            separation = Math.Abs(cB.Y) - (eA.Y + eB.X * aMB.M10 + eB.Y * aMB.M11 + eB.Z * aMB.M12);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
            {
                normal = Vector3F.UnitY;
                smallestPenetrationDepth = -separation;
                isNormalInverted         = cB.Y < 0;
                separatingAxisNumber     = 2;
            }
            #endregion

            #region ----- Case 3: Separating Axis: (0, 0, 1) -----
            separation = Math.Abs(cB.Z) - (eA.Z + eB.X * aMB.M20 + eB.Y * aMB.M21 + eB.Z * aMB.M22);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
            {
                normal = Vector3F.UnitZ;
                smallestPenetrationDepth = -separation;
                isNormalInverted         = cB.Z < 0;
                separatingAxisNumber     = 3;
            }
            #endregion

            #region ----- Case 4: Separating Axis: OrientationB * (1, 0, 0) -----
            float expression = cB.X * mB.M00 + cB.Y * mB.M10 + cB.Z * mB.M20;
            separation = Math.Abs(expression) - (eB.X + eA.X * aMB.M00 + eA.Y * aMB.M10 + eA.Z * aMB.M20);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
            {
                normal = mB.GetColumn(0);
                smallestPenetrationDepth = -separation;
                isNormalInverted         = expression < 0;
                separatingAxisNumber     = 4;
            }
            #endregion

            #region ----- Case 5: Separating Axis: OrientationB * (0, 1, 0) -----
            expression = cB.X * mB.M01 + cB.Y * mB.M11 + cB.Z * mB.M21;
            separation = Math.Abs(expression) - (eB.Y + eA.X * aMB.M01 + eA.Y * aMB.M11 + eA.Z * aMB.M21);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
            {
                normal = mB.GetColumn(1);
                smallestPenetrationDepth = -separation;
                isNormalInverted         = expression < 0;
                separatingAxisNumber     = 5;
            }
            #endregion

            #region ----- Case 6: Separating Axis: OrientationB * (0, 0, 1) -----
            expression = cB.X * mB.M02 + cB.Y * mB.M12 + cB.Z * mB.M22;
            separation = Math.Abs(expression) - (eB.Z + eA.X * aMB.M02 + eA.Y * aMB.M12 + eA.Z * aMB.M22);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth)
            {
                normal = mB.GetColumn(2);
                smallestPenetrationDepth = -separation;
                isNormalInverted         = expression < 0;
                separatingAxisNumber     = 6;
            }
            #endregion

            // The next 9 tests are edge-edge cases. The normal vector has to be normalized
            // to get the right penetration depth.
            // normal = Normalize(edgeA x edgeB)
            Vector3F separatingAxis;
            float    length;

            #region ----- Case 7: Separating Axis: (1, 0, 0) x (OrientationB * (1, 0, 0)) -----
            expression = cB.Z * mB.M10 - cB.Y * mB.M20;
            separation = Math.Abs(expression) - (eA.Y * aMB.M20 + eA.Z * aMB.M10 + eB.Y * aMB.M02 + eB.Z * aMB.M01);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(0, -mB.M20, mB.M10);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 7;
                }
            }
            #endregion

            #region ----- Case 8: Separating Axis: (1, 0, 0) x (OrientationB * (0, 1, 0)) -----
            expression = cB.Z * mB.M11 - cB.Y * mB.M21;
            separation = Math.Abs(expression) - (eA.Y * aMB.M21 + eA.Z * aMB.M11 + eB.X * aMB.M02 + eB.Z * aMB.M00);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(0, -mB.M21, mB.M11);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 8;
                }
            }
            #endregion

            #region ----- Case 9: Separating Axis: (1, 0, 0) x (OrientationB * (0, 0, 1)) -----
            expression = cB.Z * mB.M12 - cB.Y * mB.M22;
            separation = Math.Abs(expression) - (eA.Y * aMB.M22 + eA.Z * aMB.M12 + eB.X * aMB.M01 + eB.Y * aMB.M00);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(0, -mB.M22, mB.M12);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 9;
                }
            }
            #endregion

            #region ----- Case 10: Separating Axis: (0, 1, 0) x (OrientationB * (1, 0, 0)) -----
            expression = cB.X * mB.M20 - cB.Z * mB.M00;
            separation = Math.Abs(expression) - (eA.X * aMB.M20 + eA.Z * aMB.M00 + eB.Y * aMB.M12 + eB.Z * aMB.M11);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(mB.M20, 0, -mB.M00);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 10;
                }
            }
            #endregion

            #region ----- Case 11: Separating Axis: (0, 1, 0) x (OrientationB * (0, 1, 0)) -----
            expression = cB.X * mB.M21 - cB.Z * mB.M01;
            separation = Math.Abs(expression) - (eA.X * aMB.M21 + eA.Z * aMB.M01 + eB.X * aMB.M12 + eB.Z * aMB.M10);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(mB.M21, 0, -mB.M01);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 11;
                }
            }
            #endregion

            #region ----- Case 12: Separating Axis: (0, 1, 0) x (OrientationB * (0, 0, 1)) -----
            expression = cB.X * mB.M22 - cB.Z * mB.M02;
            separation = Math.Abs(expression) - (eA.X * aMB.M22 + eA.Z * aMB.M02 + eB.X * aMB.M11 + eB.Y * aMB.M10);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(mB.M22, 0, -mB.M02);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 12;
                }
            }
            #endregion

            #region ----- Case 13: Separating Axis: (0, 0, 1) x (OrientationB * (1, 0, 0)) -----
            expression = cB.Y * mB.M00 - cB.X * mB.M10;
            separation = Math.Abs(expression) - (eA.X * aMB.M10 + eA.Y * aMB.M00 + eB.Y * aMB.M22 + eB.Z * aMB.M21);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(-mB.M10, mB.M00, 0);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 13;
                }
            }
            #endregion

            #region ----- Case 14: Separating Axis: (0, 0, 1) x (OrientationB * (0, 1, 0)) -----
            expression = cB.Y * mB.M01 - cB.X * mB.M11;
            separation = Math.Abs(expression) - (eA.X * aMB.M11 + eA.Y * aMB.M01 + eB.X * aMB.M22 + eB.Z * aMB.M20);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(-mB.M11, mB.M01, 0);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 14;
                }
            }
            #endregion

            #region ----- Case 15: Separating Axis: (0, 0, 1) x (OrientationB * (0, 0, 1)) -----
            expression = cB.Y * mB.M02 - cB.X * mB.M12;
            separation = Math.Abs(expression) - (eA.X * aMB.M12 + eA.Y * aMB.M02 + eB.X * aMB.M21 + eB.Y * aMB.M20);
            if (separation > 0)
            {
                return;
            }

            if (type != CollisionQueryType.Boolean)
            {
                separatingAxis = new Vector3F(-mB.M12, mB.M02, 0);
                length         = separatingAxis.Length;
                separation    /= length;
                if (-separation < smallestPenetrationDepth)
                {
                    normal = separatingAxis / length;
                    smallestPenetrationDepth = -separation;
                    isNormalInverted         = expression < 0;
                    separatingAxisNumber     = 15;
                }
            }
            #endregion

            // We have a contact.
            contactSet.HaveContact = true;

            // HaveContact queries can exit here.
            if (type == CollisionQueryType.Boolean)
            {
                return;
            }

            // Lets find the contact info.
            Debug.Assert(smallestPenetrationDepth >= 0, "The smallest penetration depth should be greater than or equal to 0.");

            if (isNormalInverted)
            {
                normal = -normal;
            }

            // Transform normal from local space of A to world space.
            Vector3F normalWorld = poseA.ToWorldDirection(normal);

            if (separatingAxisNumber > 6)
            {
                // The intersection was detected by an edge-edge test.
                // Get the intersecting edges.
                // Separating axes:
                //  7 = x edge on A, x edge on B
                //  8 = x edge on A, y edge on B
                //  9 = x edge on A, Z edge on B
                // 10 = y edge on A, x edge on B
                // ...
                // 15 = z edge on A, z edge on B
                var edgeA = boxA.GetEdge((separatingAxisNumber - 7) / 3, normal, scaleA);
                var edgeB = boxB.GetEdge((separatingAxisNumber - 7) % 3, Matrix33F.MultiplyTransposed(mB, -normal), scaleB);
                edgeB.Start = mB * edgeB.Start + cB;
                edgeB.End   = mB * edgeB.End + cB;

                Vector3F position;
                Vector3F dummy;
                GeometryHelper.GetClosestPoints(edgeA, edgeB, out position, out dummy);
                position = position - normal * (smallestPenetrationDepth / 2); // Position is between the positions of the box surfaces.

                // Convert back position from local space of A to world space;
                position = poseA.ToWorldPosition(position);

                Contact contact = ContactHelper.CreateContact(contactSet, position, normalWorld, smallestPenetrationDepth, false);
                ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
            }
            else if (1 <= separatingAxisNumber && separatingAxisNumber <= 6)
            {
                // The intersection was detected by a face vs. * test.
                // The separating axis is perpendicular to a face.

                #region ----- Credits -----
                // The face vs. * test is based on the algorithm of the Bullet Continuous Collision
                // Detection and Physics Library. DigitalRune Geometry contains a new and improved
                // implementation of the original algorithm.
                //
                // The box-box detector in Bullet contains the following remarks:
                //
                //    Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
                //    Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
                //    All rights reserved.  Email: [email protected]   Web: www.q12.org
                //
                //    Bullet Continuous Collision Detection and Physics Library
                //    Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
                //
                //    This software is provided 'as-is', without any express or implied warranty.
                //    In no event will the authors be held liable for any damages arising from the use of this software.
                //    Permission is granted to anyone to use this software for any purpose,
                //    including commercial applications, and to alter it and redistribute it freely,
                //    subject to the following restrictions:
                //
                //    1. The origin of this software must not be misrepresented; you must not claim that you wrote the
                //       original software. If you use this software in a product, an acknowledgment in the product
                //       documentation would be appreciated but is not required.
                //    2. Altered source versions must be plainly marked as such, and must not be misrepresented as being
                //       the original software.
                //    3. This notice may not be removed or altered from any source distribution.
                #endregion

                // We define the face perpendicular to the separating axis to be the "reference face".
                // The face of the other box closest to the reference face is called the "incident face".
                // Accordingly, we will call the box containing the reference face the "reference box" and
                // the box containing the incident face the "incident box".
                //
                // We will transform the incident face into the 2D space of reference face. Then we will
                // clip the incident face against the reference face. The polygon resulting from the
                // intersection will be transformed back into world space and the points of the polygon will
                // be the candidates for the contact points.

                Pose     poseR;      // Pose of reference box.
                Pose     poseI;      // Pose of incident box.
                Vector3F boxExtentR; // Half extent of reference box.
                Vector3F boxExtentI; // Half extent of incident box.

                // Contact normal (= normal of reference face) in world space.
                if (separatingAxisNumber <= 3)
                {
                    poseR            = poseA;
                    poseI            = poseB;
                    boxExtentR       = eA;
                    boxExtentI       = eB;
                    isNormalInverted = false;
                }
                else
                {
                    poseR            = poseB;
                    poseI            = poseA;
                    boxExtentR       = eB;
                    boxExtentI       = eA;
                    normalWorld      = -normalWorld;
                    isNormalInverted = true;
                }

                // Contact normal in local space of incident box.
                Vector3F normalI = poseI.ToLocalDirection(normalWorld);

                Vector3F absNormal = normalI;
                absNormal.Absolute();

                Vector3F xAxisInc, yAxisInc; // The basis of the incident-face space.
                float    absFaceOffsetI;     // The offset of the incident face to the center of the box.
                Vector2F faceExtentI;        // The half extent of the incident face.
                Vector3F faceNormal;         // The normal of the incident face in world space.
                float    faceDirection;      // A value indicating the direction of the incident face.

                // Find the largest component of the normal. The largest component indicates which face is
                // the incident face.
                switch (Vector3F.Absolute(normalI).IndexOfLargestComponent)
                {
                case 0:
                    faceExtentI.X  = boxExtentI.Y;
                    faceExtentI.Y  = boxExtentI.Z;
                    absFaceOffsetI = boxExtentI.X;
                    faceNormal     = poseI.Orientation.GetColumn(0);
                    xAxisInc       = poseI.Orientation.GetColumn(1);
                    yAxisInc       = poseI.Orientation.GetColumn(2);
                    faceDirection  = normalI.X;
                    break;

                case 1:
                    faceExtentI.X  = boxExtentI.X;
                    faceExtentI.Y  = boxExtentI.Z;
                    absFaceOffsetI = boxExtentI.Y;
                    faceNormal     = poseI.Orientation.GetColumn(1);
                    xAxisInc       = poseI.Orientation.GetColumn(0);
                    yAxisInc       = poseI.Orientation.GetColumn(2);
                    faceDirection  = normalI.Y;
                    break;

                // case 2:
                default:
                    faceExtentI.X  = boxExtentI.X;
                    faceExtentI.Y  = boxExtentI.Y;
                    absFaceOffsetI = boxExtentI.Z;
                    faceNormal     = poseI.Orientation.GetColumn(2);
                    xAxisInc       = poseI.Orientation.GetColumn(0);
                    yAxisInc       = poseI.Orientation.GetColumn(1);
                    faceDirection  = normalI.Z;
                    break;
                }

                // Compute center of incident face relative to the center of the reference box in world space.
                float    faceOffset    = (faceDirection < 0) ? absFaceOffsetI : -absFaceOffsetI;
                Vector3F centerOfFaceI = faceNormal * faceOffset + poseI.Position - poseR.Position;

                // (Note: We will use the center of the incident face to compute the points of the incident
                // face and transform the points into the reference-face frame. The center of the incident
                // face is relative to the center of the reference box. We could also get center of the
                // incident face relative to the center of the reference face. But since we are projecting
                // the points from 3D to 2D this does not matter.)

                Vector3F xAxisR, yAxisR; // The basis of the reference-face space.
                float    faceOffsetR;    // The offset of the reference face to the center of the box.
                Vector2F faceExtentR;    // The half extent of the reference face.
                switch (separatingAxisNumber)
                {
                case 1:
                case 4:
                    faceExtentR.X = boxExtentR.Y;
                    faceExtentR.Y = boxExtentR.Z;
                    faceOffsetR   = boxExtentR.X;
                    xAxisR        = poseR.Orientation.GetColumn(1);
                    yAxisR        = poseR.Orientation.GetColumn(2);
                    break;

                case 2:
                case 5:
                    faceExtentR.X = boxExtentR.X;
                    faceExtentR.Y = boxExtentR.Z;
                    faceOffsetR   = boxExtentR.Y;
                    xAxisR        = poseR.Orientation.GetColumn(0);
                    yAxisR        = poseR.Orientation.GetColumn(2);
                    break;

                // case 3:
                // case 6:
                default:
                    faceExtentR.X = boxExtentR.X;
                    faceExtentR.Y = boxExtentR.Y;
                    faceOffsetR   = boxExtentR.Z;
                    xAxisR        = poseR.Orientation.GetColumn(0);
                    yAxisR        = poseR.Orientation.GetColumn(1);
                    break;
                }

                // Compute the center of the incident face in the reference-face frame.
                // We can simply project centerOfFaceI onto the x- and y-axis of the reference
                // face.
                Vector2F centerOfFaceIInR;

                //centerOfFaceIInR.X = Vector3F.Dot(centerOfFaceI, xAxisR);
                // ----- Optimized version:
                centerOfFaceIInR.X = centerOfFaceI.X * xAxisR.X + centerOfFaceI.Y * xAxisR.Y + centerOfFaceI.Z * xAxisR.Z;

                //centerOfFaceIInR.Y = Vector3F.Dot(centerOfFaceI, yAxisR);
                // ----- Optimized version:
                centerOfFaceIInR.Y = centerOfFaceI.X * yAxisR.X + centerOfFaceI.Y * yAxisR.Y + centerOfFaceI.Z * yAxisR.Z;

                // Now, we have the center of the incident face in reference-face coordinates.
                // To compute the corners of the incident face in reference-face coordinates, we need
                // transform faceExtentI (the half extent vector of the incident face) from the incident-
                // face frame to the reference-face frame to compute the corners.
                //
                // The reference-face frame has the basis
                //   mR = (xAxisR, yAxisR, ?)
                //
                // The incident-face frame has the basis
                //   mI = (xAxisI, yAxisI, ?)
                //
                // Rotation from incident-face frame to reference-face frame is
                //   mIToR = mR^-1 * mI
                //
                // The corner offsets in incident-face space is are vectors (x, y, 0). To transform these
                // vectors from incident-face space to reference-face space we need to calculate:
                //   mIToR * v
                //
                // Since the z-components are 0 and we are only interested in the resulting x, y coordinates
                // in reference-space when can reduce the rotation to a 2 x 2 matrix. (The other components
                // are not needed.)

                // ----- Optimized version: (Original on the right)
                Matrix22F mIToR;
                mIToR.M00 = xAxisR.X * xAxisInc.X + xAxisR.Y * xAxisInc.Y + xAxisR.Z * xAxisInc.Z; // mIToR.M00 = Vector3F.Dot(xAxisR, xAxisInc);
                mIToR.M01 = xAxisR.X * yAxisInc.X + xAxisR.Y * yAxisInc.Y + xAxisR.Z * yAxisInc.Z; // mIToR.M01 = Vector3F.Dot(xAxisR, yAxisInc);
                mIToR.M10 = yAxisR.X * xAxisInc.X + yAxisR.Y * xAxisInc.Y + yAxisR.Z * xAxisInc.Z; // mIToR.M10 = Vector3F.Dot(yAxisR, xAxisInc);
                mIToR.M11 = yAxisR.X * yAxisInc.X + yAxisR.Y * yAxisInc.Y + yAxisR.Z * yAxisInc.Z; // mIToR.M11 = Vector3F.Dot(yAxisR, yAxisInc);

                // The corner offsets in incident-face space are:
                //  (-faceExtentI.X, -faceExtentI.Y) ... left, bottom corner
                //  ( faceExtentI.X, -faceExtentI.Y) ... right, bottom corner
                //  ( faceExtentI.X,  faceExtentI.Y) ... right, top corner
                //  (-faceExtentI.X,  faceExtentI.Y) ... left, top corner
                //
                // Instead of transforming each corner offset, we can optimize the computation: Do the
                // matrix-vector multiplication once, keep the intermediate products, apply the sign
                // of the components when adding the intermediate results.

                float           k1   = mIToR.M00 * faceExtentI.X; // Products of matrix-vector multiplication.
                float           k2   = mIToR.M01 * faceExtentI.Y;
                float           k3   = mIToR.M10 * faceExtentI.X;
                float           k4   = mIToR.M11 * faceExtentI.Y;
                List <Vector2F> quad = DigitalRune.ResourcePools <Vector2F> .Lists.Obtain();

                quad.Add(new Vector2F(centerOfFaceIInR.X - k1 - k2, centerOfFaceIInR.Y - k3 - k4));
                quad.Add(new Vector2F(centerOfFaceIInR.X + k1 - k2, centerOfFaceIInR.Y + k3 - k4));
                quad.Add(new Vector2F(centerOfFaceIInR.X + k1 + k2, centerOfFaceIInR.Y + k3 + k4));
                quad.Add(new Vector2F(centerOfFaceIInR.X - k1 + k2, centerOfFaceIInR.Y - k3 + k4));

                // Clip incident face (quadrilateral) against reference face (rectangle).
                List <Vector2F> contacts2D = ClipQuadrilateralAgainstRectangle(faceExtentR, quad);

                // Transform contact points back to world space and compute penetration depths.
                int             numberOfContacts = contacts2D.Count;
                List <Vector3F> contacts3D       = DigitalRune.ResourcePools <Vector3F> .Lists.Obtain();

                List <float> penetrationDepths = DigitalRune.ResourcePools <float> .Lists.Obtain();

                Matrix22F mRToI = mIToR.Inverse;
                for (int i = numberOfContacts - 1; i >= 0; i--)
                {
                    Vector2F contact2DR = contacts2D[i];                           // Contact in reference-face space.
                    Vector2F contact2DI = mRToI * (contact2DR - centerOfFaceIInR); // Contact in incident-face space.

                    // Transform point in incident-face space to world (relative to center of reference box).
                    // contact3D = mI * (x, y, 0) + centerOfFaceI
                    Vector3F contact3D;
                    contact3D.X = xAxisInc.X * contact2DI.X + yAxisInc.X * contact2DI.Y + centerOfFaceI.X;
                    contact3D.Y = xAxisInc.Y * contact2DI.X + yAxisInc.Y * contact2DI.Y + centerOfFaceI.Y;
                    contact3D.Z = xAxisInc.Z * contact2DI.X + yAxisInc.Z * contact2DI.Y + centerOfFaceI.Z;

                    // Compute penetration depth.

                    //float penetrationDepth = faceOffsetR - Vector3F.Dot(normalWorld, contact3D);
                    // ----- Optimized version:
                    float penetrationDepth = faceOffsetR - (normalWorld.X * contact3D.X + normalWorld.Y * contact3D.Y + normalWorld.Z * contact3D.Z);

                    if (penetrationDepth >= 0)
                    {
                        // Valid contact.
                        contacts3D.Add(contact3D);
                        penetrationDepths.Add(penetrationDepth);
                    }
                    else
                    {
                        // Remove bad contacts from the 2D contacts.
                        // (We might still need the 2D contacts, if we need to reduce the contacts.)
                        contacts2D.RemoveAt(i);
                    }
                }

                numberOfContacts = contacts3D.Count;
                if (numberOfContacts == 0)
                {
                    return; // Should never happen.
                }
                // Revert normal back to original direction.
                normal = (isNormalInverted) ? -normalWorld : normalWorld;

                // Note: normal ........ contact normal pointing from box A to B.
                //       normalWorld ... contact normal pointing from reference box to incident box.

                if (numberOfContacts <= MaxNumberOfContacts)
                {
                    // Add all contacts to contact set.
                    for (int i = 0; i < numberOfContacts; i++)
                    {
                        float penetrationDepth = penetrationDepths[i];

                        // Position is between the positions of the box surfaces.
                        Vector3F position = contacts3D[i] + poseR.Position + normalWorld * (penetrationDepth / 2);

                        Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
                        ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
                    }
                }
                else
                {
                    // Reduce number of contacts, keep the contact with the max penetration depth.
                    int   indexOfDeepest      = 0;
                    float maxPenetrationDepth = penetrationDepths[0];
                    for (int i = 1; i < numberOfContacts; i++)
                    {
                        float penetrationDepth = penetrationDepths[i];
                        if (penetrationDepth > maxPenetrationDepth)
                        {
                            maxPenetrationDepth = penetrationDepth;
                            indexOfDeepest      = i;
                        }
                    }

                    List <int> indicesOfContacts = ReduceContacts(contacts2D, indexOfDeepest, MaxNumberOfContacts);

                    // Add selected contacts to contact set.
                    numberOfContacts = indicesOfContacts.Count;
                    for (int i = 0; i < numberOfContacts; i++)
                    {
                        int   index            = indicesOfContacts[i];
                        float penetrationDepth = penetrationDepths[index];

                        // Position is between the positions of the box surfaces.
                        Vector3F position = contacts3D[index] + poseR.Position + normalWorld * (penetrationDepth / 2);

                        Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepths[index], false);
                        ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
                    }

                    DigitalRune.ResourcePools <int> .Lists.Recycle(indicesOfContacts);
                }

                DigitalRune.ResourcePools <Vector2F> .Lists.Recycle(contacts2D);

                DigitalRune.ResourcePools <Vector3F> .Lists.Recycle(contacts3D);

                DigitalRune.ResourcePools <float> .Lists.Recycle(penetrationDepths);
            }
        }
Ejemplo n.º 45
0
        private void AddContact(ContactSet contactSet, 
            bool swapped,
            CollisionQueryType type,
            ref Ray rayWorld,             // The ray in world space.
            ref Ray rayInMesh,            // The ray in the scaled triangle mesh space.
            ref Triangle triangle,        // The unscaled triangle in the mesh space.
            int triangleIndex,
            ref Pose trianglePose,
            ref Vector3F triangleScale,
            bool isTwoSided)
        {
            // This code is from GeometryHelper_Triangles.cs. Sync changes!

              Vector3F v0 = triangle.Vertex0 * triangleScale;
              Vector3F v1 = triangle.Vertex1 * triangleScale;
              Vector3F v2 = triangle.Vertex2 * triangleScale;

              Vector3F d1 = (v1 - v0);
              Vector3F d2 = (v2 - v0);
              Vector3F n = Vector3F.Cross(d1, d2);

              // Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments".
              float ε = n.Length * Numeric.EpsilonFSquared;

              Vector3F r = rayInMesh.Direction * rayInMesh.Length;

              float δ = -Vector3F.Dot(r, n);

              // Degenerate triangle --> No hit.
              if (ε == 0.0f || Numeric.IsZero(δ, ε))
            return;

              Vector3F triangleToRayOrigin = rayInMesh.Origin - v0;
              float λ = Vector3F.Dot(triangleToRayOrigin, n) / δ;
              if (λ < 0 || λ > 1)
            return;

              // The ray hit the triangle plane.
              Vector3F u = Vector3F.Cross(triangleToRayOrigin, r);
              float μ1 = Vector3F.Dot(d2, u) / δ;
              float μ2 = Vector3F.Dot(-d1, u) / δ;
              if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε)
              {
            // Hit!
            contactSet.HaveContact = true;

            if (type == CollisionQueryType.Boolean)
              return;

            if (δ < 0 && !isTwoSided)
              return;   // Shooting into the back of a one-sided triangle - no contact.

            float penetrationDepth = λ * rayInMesh.Length;

            // Create contact info.
            Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
            n = trianglePose.ToWorldDirection(n);

            Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above.");
            n.Normalize();

            if (δ < 0)
              n = -n;

            if (swapped)
              n = -n;

            Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true);

            if (swapped)
              contact.FeatureB = triangleIndex;
            else
              contact.FeatureA = triangleIndex;

            Debug.Assert(
              contactSet.ObjectA.GeometricObject.Shape is RayShape && contact.FeatureA == -1 ||
              contactSet.ObjectB.GeometricObject.Shape is RayShape && contact.FeatureB == -1,
              "RayTriangleMeshAlgorithm has set the wrong feature property.");

            ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
              }
        }
Ejemplo n.º 46
0
        public static void Merge(ContactSet contactSet, Contact newContact, CollisionQueryType type, float contactPositionTolerance)
        {
            Debug.Assert(contactSet != null);
              Debug.Assert(newContact != null);

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

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

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

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

              // ----- Default: Add the new contact.
              contactSet.Add(newContact);
        }
Ejemplo n.º 47
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            // From Coutinho: "Dynamic Simulations of Multibody Systems" and
              // Bergen: "Collision Detection in Interactive 3D Environments".

              // Object A should be the box.
              // Object B should be the sphere.
              IGeometricObject boxObject = contactSet.ObjectA.GeometricObject;
              IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject;

              // Swap objects if necessary.
              bool swapped = (sphereObject.Shape is BoxShape);
              if (swapped)
            MathHelper.Swap(ref boxObject, ref sphereObject);

              BoxShape boxShape = boxObject.Shape as BoxShape;
              SphereShape sphereShape = sphereObject.Shape as SphereShape;

              // Check if collision objects shapes are correct.
              if (boxShape == null || sphereShape == null)
            throw new ArgumentException("The contact set must contain a box and a sphere.", "contactSet");

              Vector3F scaleBox = Vector3F.Absolute(boxObject.Scale);
              Vector3F scaleSphere = Vector3F.Absolute(sphereObject.Scale);

              // Call other algorithm for non-uniformly scaled spheres.
              if (scaleSphere.X != scaleSphere.Y || scaleSphere.Y != scaleSphere.Z)
              {
            if (_fallbackAlgorithm == null)
              _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(BoxShape), typeof(ConvexShape)];

            _fallbackAlgorithm.ComputeCollision(contactSet, type);
            return;
              }

              // Apply scale.
              Vector3F boxExtent = boxShape.Extent * scaleBox;
              float sphereRadius = sphereShape.Radius * scaleSphere.X;

              // ----- First transform sphere center into the local space of the box.
              Pose boxPose = boxObject.Pose;
              Vector3F sphereCenterWorld = sphereObject.Pose.Position;
              Vector3F sphereCenter = boxPose.ToLocalPosition(sphereCenterWorld);

              Vector3F p = Vector3F.Zero;
              bool sphereCenterIsContainedInBox = true;

              // When sphere center is on a box surface we have to choose a suitable normal.
              // otherwise the normal will be computed later.
              Vector3F normal = Vector3F.Zero;

              #region ----- Look for the point p of the box that is closest to center of the sphere. -----
              Vector3F boxHalfExtent = 0.5f * boxExtent;

              // x component
              if (sphereCenter.X < -boxHalfExtent.X)
              {
            p.X = -boxHalfExtent.X;
            sphereCenterIsContainedInBox = false;
              }
              else if (sphereCenter.X > boxHalfExtent.X)
              {
            p.X = boxHalfExtent.X;
            sphereCenterIsContainedInBox = false;
              }
              else
              {
            p.X = sphereCenter.X;
              }

              // y component
              if (sphereCenter.Y < -boxHalfExtent.Y)
              {
            p.Y = -boxHalfExtent.Y;
            sphereCenterIsContainedInBox = false;
              }
              else if (sphereCenter.Y > boxHalfExtent.Y)
              {
            p.Y = boxHalfExtent.Y;
            sphereCenterIsContainedInBox = false;
              }
              else
              {
            p.Y = sphereCenter.Y;
              }

              // z component
              if (sphereCenter.Z < -boxHalfExtent.Z)
              {
            p.Z = -boxHalfExtent.Z;
            sphereCenterIsContainedInBox = false;
              }
              else if (sphereCenter.Z > boxHalfExtent.Z)
              {
            p.Z = boxHalfExtent.Z;
            sphereCenterIsContainedInBox = false;
              }
              else
              {
            p.Z = sphereCenter.Z;
              }

              if (sphereCenterIsContainedInBox || (sphereCenter - p).IsNumericallyZero)
              {
            // Special case: Sphere center is within box. In this case p == center.
            // Lets return a point on the surface of the box.
            // Lets find the axis with the smallest way out (penetration depth).
            Vector3F diff = boxHalfExtent - Vector3F.Absolute(sphereCenter);
            if (diff.X <= diff.Y && diff.X <= diff.Z)
            {
              // Point on one of the x surfaces is nearest.
              // Check whether positive or negative surface.
              bool positive = (sphereCenter.X > 0);
              p.X = positive ? boxHalfExtent.X : -boxHalfExtent.X;

              if (Numeric.IsZero(diff.X))
              {
            // Sphere center is on box surface.
            normal = positive ? Vector3F.UnitX : -Vector3F.UnitX;
              }
            }
            else if (diff.Y <= diff.X && diff.Y <= diff.Z)
            {
              // Point on one of the y surfaces is nearest.
              // Check whether positive or negative surface.
              bool positive = (sphereCenter.Y > 0);
              p.Y = positive ? boxHalfExtent.Y : -boxHalfExtent.Y;

              if (Numeric.IsZero(diff.Y))
              {
            // Sphere center is on box surface.
            normal = positive ? Vector3F.UnitY : -Vector3F.UnitY;
              }
            }
            else
            {
              // Point on one of the z surfaces is nearest.
              // Check whether positive or negative surface.
              bool positive = (sphereCenter.Z > 0);
              p.Z = positive ? boxHalfExtent.Z : -boxHalfExtent.Z;

              if (Numeric.IsZero(diff.Z))
              {
            // Sphere center is on box surface.
            normal = positive ? Vector3F.UnitZ : -Vector3F.UnitZ;
              }
            }
              }
              #endregion

              // ----- Convert back to world space
              p = boxPose.ToWorldPosition(p);
              Vector3F sphereCenterToP = p - sphereCenterWorld;

              // Compute penetration depth.
              float penetrationDepth = sphereCenterIsContainedInBox
                                 ? sphereRadius + sphereCenterToP.Length
                                 : sphereRadius - sphereCenterToP.Length;
              contactSet.HaveContact = (penetrationDepth >= 0);

              if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
              {
            // HaveContact queries can exit here.
            // GetContacts queries can exit here if we don't have a contact.
            return;
              }

              // ----- Create collision info.
              // Compute normal if we haven't set one yet.
              if (normal == Vector3F.Zero)
              {
            Debug.Assert(!sphereCenterToP.IsNumericallyZero, "When the center of the sphere lies on the box surface a normal should be have been set explicitly.");
            normal = sphereCenterIsContainedInBox ? sphereCenterToP : -sphereCenterToP;
            normal.Normalize();
              }
              else
              {
            normal = boxPose.ToWorldDirection(normal);
              }

              // Position = point between sphere and box surface.
              Vector3F position = p - normal * (penetrationDepth / 2);
              if (swapped)
            normal = -normal;

              // Update contact set.
              Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
              ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
        }
Ejemplo n.º 48
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);
      }
    }
Ejemplo n.º 49
0
        /// <summary>
        /// Computes the collision between line vs. line.
        /// </summary>
        /// <param name="contactSet">The contact set.</param>
        /// <param name="type">The type of collision query.</param>
        private void ComputeLineVsLine(ContactSet contactSet, CollisionQueryType type)
        {
            IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
            IGeometricObject objectB = contactSet.ObjectB.GeometricObject;

            Debug.Assert(objectA.Shape is LineShape && objectB.Shape is LineShape, "LineAlgorithm.ComputeLineVsLine should only be called for 2 line shapes.");
            Debug.Assert(contactSet.Count <= 1, "Two lines should have at max 1 contact point.");

            // Get transformations.
            Vector3F scaleA = objectA.Scale;
            Vector3F scaleB = objectB.Scale;
            Pose     poseA  = objectA.Pose;
            Pose     poseB  = objectB.Pose;

            // Create two line objects in world space.
            var lineA = new Line((LineShape)objectA.Shape);

            lineA.Scale(ref scaleA);
            lineA.ToWorld(ref poseA);

            var lineB = new Line((LineShape)objectB.Shape);

            lineB.Scale(ref scaleB);
            lineB.ToWorld(ref poseB);

            // Get closest points.
            Vector3F pointA;
            Vector3F pointB;

            contactSet.HaveContact = GeometryHelper.GetClosestPoints(lineA, lineB, out pointA, out pointB);

            if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
            {
                // HaveContact queries can exit here.
                // GetContacts queries can exit here if we don't have a contact.
                return;
            }

            // Create contact information.
            Vector3F position = (pointA + pointB) / 2;
            Vector3F normal   = pointB - pointA;
            float    length   = normal.Length;

            if (Numeric.IsZero(length))
            {
                // Create normal from cross product of both lines.
                normal = Vector3F.Cross(lineA.Direction, lineB.Direction);
                if (!normal.TryNormalize())
                {
                    normal = Vector3F.UnitY;
                }
            }
            else
            {
                // Normalize vector
                normal = normal / length;
            }

            Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -length, false);

            ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
        }
Ejemplo n.º 50
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);
      }
    }
Ejemplo n.º 51
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            // Object A should be the plane.
            // Object B should be the other object.
            IGeometricObject planeObject  = contactSet.ObjectA.GeometricObject;
            IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;

            // Swap objects if necessary.
            bool swapped = (convexObject.Shape is PlaneShape);

            if (swapped)
            {
                MathHelper.Swap(ref planeObject, ref convexObject);
            }

            PlaneShape  planeShape  = planeObject.Shape as PlaneShape;
            ConvexShape convexShape = convexObject.Shape as ConvexShape;

            // Check if shapes are correct.
            if (planeShape == null || convexShape == null)
            {
                throw new ArgumentException("The contact set must contain a plane and a convex shape.", "contactSet");
            }

            // Get transformations.
            Vector3F scalePlane = planeObject.Scale;
            Vector3F scaleB     = convexObject.Scale;
            Pose     planePose  = planeObject.Pose;
            Pose     poseB      = convexObject.Pose;

            // Apply scale to plane and transform plane into world space.
            Plane planeWorld = new Plane(planeShape);

            planeWorld.Scale(ref scalePlane);   // Scale plane.
            planeWorld.ToWorld(ref planePose);  // Transform plane to world space.

            // Transform plane normal to local space of convex.
            Vector3F planeNormalLocalB = poseB.ToLocalDirection(planeWorld.Normal);

            // Get support vertex nearest to the plane.
            Vector3F supportVertexBLocal = convexShape.GetSupportPoint(-planeNormalLocalB, scaleB);

            // Transform support vertex into world space.
            Vector3F supportVertexBWorld = poseB.ToWorldPosition(supportVertexBLocal);

            // Project vertex onto separating axis (given by plane normal).
            float distance = Vector3F.Dot(supportVertexBWorld, planeWorld.Normal);

            // Check for collision.
            float penetrationDepth = planeWorld.DistanceFromOrigin - distance;

            contactSet.HaveContact = (penetrationDepth >= 0);

            if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
            {
                // HaveContact queries can exit here.
                // GetContacts queries can exit here if we don't have a contact.
                return;
            }

            // Position is between support vertex and plane.
            Vector3F position = supportVertexBWorld + planeWorld.Normal * (penetrationDepth / 2);
            Vector3F normal   = (swapped) ? -planeWorld.Normal : planeWorld.Normal;

            // Update contact set.
            Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);

            ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);

            if (CollisionDetection.FullContactSetPerFrame &&
                type == CollisionQueryType.Contacts &&
                contactSet.Count > 0 &&
                contactSet.Count < 4)
            {
                // Special treatment for tetrahedra: Test all vertices against plane.
                IList <Vector3F> vertices = null;
                if (convexShape is ConvexHullOfPoints)
                {
                    var convexHullOfPoints = (ConvexHullOfPoints)convexShape;
                    vertices = convexHullOfPoints.Points;
                }
                else if (convexShape is ConvexPolyhedron)
                {
                    var convexPolyhedron = (ConvexPolyhedron)convexShape;
                    vertices = convexPolyhedron.Vertices;
                }

                if (vertices != null && vertices.Count <= 8)
                {
                    // Convex has 8 or less vertices. Explicitly test all vertices against the plane.
                    int numberOfVertices = vertices.Count;
                    for (int i = 0; i < numberOfVertices; i++)
                    {
                        // Test is the same as above.
                        var      vertex       = vertices[i];
                        Vector3F scaledVertex = vertex * scaleB;
                        if (scaledVertex != supportVertexBLocal) // supportVertexBLocal has already been added.
                        {
                            Vector3F vertexWorld = poseB.ToWorldPosition(scaledVertex);
                            distance         = Vector3F.Dot(vertexWorld, planeWorld.Normal);
                            penetrationDepth = planeWorld.DistanceFromOrigin - distance;
                            if (penetrationDepth >= 0)
                            {
                                position = vertexWorld + planeWorld.Normal * (penetrationDepth / 2);
                                normal   = (swapped) ? -planeWorld.Normal : planeWorld.Normal;
                                contact  = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
                                ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
                            }
                        }
                    }
                }
                else
                {
                    // Convex is a complex shape with more than 4 vertices.
                    ContactHelper.TestWithPerturbations(
                        CollisionDetection,
                        contactSet,
                        !swapped, // Perturb the convex object, not the plane.
                        _computeContactsMethod);
                }
            }
        }
Ejemplo n.º 52
0
    public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
    {
      CollisionObject collisionObjectA = contactSet.ObjectA;
      CollisionObject collisionObjectB = contactSet.ObjectB;
      IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
      IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;

      // Object A should be the composite, swap objects if necessary.
      // When testing CompositeShape vs. CompositeShape with BVH, object A should be the 
      // CompositeShape with BVH.
      CompositeShape compositeShapeA = geometricObjectA.Shape as CompositeShape;
      CompositeShape compositeShapeB = geometricObjectB.Shape as CompositeShape;
      bool swapped = false;
      if (compositeShapeA == null)
      {
        // Object A is something else. Object B must be a composite shape.
        swapped = true;
      }
      else if (compositeShapeA.Partition == null 
               && compositeShapeB != null 
               && compositeShapeB.Partition != null)
      {
        // Object A has no BVH, object B is CompositeShape with BVH.
        swapped = true;
      }

      if (swapped)
      {
        MathHelper.Swap(ref collisionObjectA, ref collisionObjectB);
        MathHelper.Swap(ref geometricObjectA, ref geometricObjectB);
        MathHelper.Swap(ref compositeShapeA, ref compositeShapeB);
      }

      // Check if collision objects shapes are correct.
      if (compositeShapeA == null)
        throw new ArgumentException("The contact set must contain a composite shape.", "contactSet");

      // Assume no contact.
      contactSet.HaveContact = false;

      Vector3F scaleA = geometricObjectA.Scale;
      Vector3F scaleB = geometricObjectB.Scale;

      // Check if transforms are supported.
      if (compositeShapeA != null                                           // When object A is a CompositeShape
          && (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z)                 // non-uniform scaling is not supported
          && compositeShapeA.Children.Any(child => child.Pose.HasRotation)) // when a child has a local rotation.
      {                                                                     // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
        throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
      }

      // Same check for object B.
      if (compositeShapeB != null
          && (scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z)
          && compositeShapeB.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
      {
        throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
      }

      // ----- A few fixed objects which are reused to avoid GC garbage.
      var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
      var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();

      // Create a test contact set and initialize with dummy objects.
      // (The actual collision objects are set below.)
      var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
      var testGeometricObjectA = TestGeometricObject.Create();
      var testGeometricObjectB = TestGeometricObject.Create();

      try
      {
        if (compositeShapeA.Partition != null
            && (type != CollisionQueryType.ClosestPoints
                || compositeShapeA.Partition is ISupportClosestPointQueries<int>))
        {
          if (compositeShapeB != null && compositeShapeB.Partition != null)
          {
            #region ----- Composite with BVH vs. Composite with BVH -----

            Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine.");

            if (type != CollisionQueryType.ClosestPoints)
            {
              // ----- Boolean or Contact Query

              // Heuristic: Test large BVH vs. small BVH.
              Aabb aabbOfA = geometricObjectA.Aabb;
              Aabb aabbOfB = geometricObjectB.Aabb;
              float largestExtentA = aabbOfA.Extent.LargestComponent;
              float largestExtentB = aabbOfB.Extent.LargestComponent;
              IEnumerable<Pair<int>> overlaps;
              bool overlapsSwapped = largestExtentA < largestExtentB;
              if (overlapsSwapped)
              {
                overlaps = compositeShapeB.Partition.GetOverlaps(
                  scaleB,
                  geometricObjectB.Pose,
                  compositeShapeA.Partition,
                  scaleA,
                  geometricObjectA.Pose);
              }
              else
              {
                overlaps = compositeShapeA.Partition.GetOverlaps(
                  scaleA,
                  geometricObjectA.Pose,
                  compositeShapeB.Partition,
                  scaleB,
                  geometricObjectB.Pose);
              }

              foreach (var overlap in overlaps)
              {
                if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
                  break; // We can abort early.

                AddChildChildContacts(
                  contactSet,
                  overlapsSwapped ? overlap.Second : overlap.First,
                  overlapsSwapped ? overlap.First : overlap.Second,
                  type,
                  testContactSet,
                  testCollisionObjectA,
                  testGeometricObjectA,
                  testCollisionObjectB,
                  testGeometricObjectB);
              }
            }
            else
            {
              // Closest-Point Query

              var callback = ClosestPointsCallbacks.Obtain();
              callback.CollisionAlgorithm = this;
              callback.Swapped = false;
              callback.ContactSet = contactSet;
              callback.TestCollisionObjectA = testCollisionObjectA;
              callback.TestCollisionObjectB = testCollisionObjectB;
              callback.TestGeometricObjectA = testGeometricObjectA;
              callback.TestGeometricObjectB = testGeometricObjectB;
              callback.TestContactSet = testContactSet;

              ((ISupportClosestPointQueries<int>)compositeShapeA.Partition)
                .GetClosestPointCandidates(
                  scaleA,
                  geometricObjectA.Pose,
                  compositeShapeB.Partition,
                  scaleB,
                  geometricObjectB.Pose,
                  callback.HandlePair);

              ClosestPointsCallbacks.Recycle(callback);
            }
            #endregion
          }
          else
          {
            #region ----- Composite with BVH vs. * -----

            // Compute AABB of B in local space of the CompositeShape.
            Aabb aabbBInA = geometricObjectB.Shape.GetAabb(
              scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);

            // Apply inverse scaling to do the AABB checks in the unscaled local space of A.
            aabbBInA.Scale(Vector3F.One / scaleA);

            if (type != CollisionQueryType.ClosestPoints)
            {
              // Boolean or Contact Query

              foreach (var childIndex in compositeShapeA.Partition.GetOverlaps(aabbBInA))
              {
                if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
                  break; // We can abort early.

                AddChildContacts(
                  contactSet, 
                  swapped, 
                  childIndex, 
                  type, 
                  testContactSet, 
                  testCollisionObjectA,
                  testGeometricObjectA);
              }
            }
            else if (type == CollisionQueryType.ClosestPoints)
            {
              // Closest-Point Query

              var callback = ClosestPointsCallbacks.Obtain();
              callback.CollisionAlgorithm = this;
              callback.Swapped = swapped;
              callback.ContactSet = contactSet;
              callback.TestCollisionObjectA = testCollisionObjectA;
              callback.TestCollisionObjectB = testCollisionObjectB;
              callback.TestGeometricObjectA = testGeometricObjectA;
              callback.TestGeometricObjectB = testGeometricObjectB;
              callback.TestContactSet = testContactSet;

              ((ISupportClosestPointQueries<int>)compositeShapeA.Partition)
                .GetClosestPointCandidates(
                  aabbBInA,
                  float.PositiveInfinity,
                  callback.HandleItem);

              ClosestPointsCallbacks.Recycle(callback);
            }
            #endregion
          }
        }
        else
        {
          #region ----- Composite vs. *-----

          // Compute AABB of B in local space of the composite.
          Aabb aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
          
          // Apply inverse scaling to do the AABB checks in the unscaled local space of A.
          aabbBInA.Scale(Vector3F.One / scaleA);

          // Go through list of children and find contacts.
          int numberOfChildGeometries = compositeShapeA.Children.Count;
          for (int i = 0; i < numberOfChildGeometries; i++)
          {
            IGeometricObject child = compositeShapeA.Children[i];

            // NOTE: For closest-point queries we could be faster estimating a search space.
            // See TriangleMeshAlgorithm or BVH queries.
            // But the current implementation is sufficient. If the CompositeShape is more complex
            // the user should be using spatial partitions anyway.

            // For boolean or contact queries, we make an AABB test first.
            // For closest points where we have not found a contact yet, we have to search
            // all children.
            if ((type == CollisionQueryType.ClosestPoints && !contactSet.HaveContact)
                || GeometryHelper.HaveContact(aabbBInA, child.Shape.GetAabb(child.Scale, child.Pose)))
            {
              // TODO: We could compute the minDistance of the child AABB and the AABB of the
              // other shape. If the minDistance is greater than the current closestPairDistance
              // we can ignore this pair. - This could be a performance boost.

              // Get contacts/closest pairs of this child.
              AddChildContacts(
                contactSet, 
                swapped, 
                i, 
                type, 
                testContactSet,
                testCollisionObjectA,
                testGeometricObjectA);

              // We have contact and stop for boolean queries.
              if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
                break;
            }
          }
          #endregion
        }
      }
      finally
      {
        Debug.Assert(collisionObjectA.GeometricObject.Shape == compositeShapeA, "Shape was altered and not restored.");

        testContactSet.Recycle();
        ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
        ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
        testGeometricObjectB.Recycle();
        testGeometricObjectA.Recycle();
      }
    }
Ejemplo n.º 53
0
        private Vector3 DoMpr(CollisionQueryType type, ContactSet contactSet, Vector3 v0)
        {
            int       iterationCount = 0;
            const int iterationLimit = 100;

            CollisionObject  collisionObjectA = contactSet.ObjectA;
            IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
            ConvexShape      shapeA           = (ConvexShape)geometricObjectA.Shape;
            Vector3          scaleA           = geometricObjectA.Scale;
            Pose             poseA            = geometricObjectA.Pose;

            CollisionObject  collisionObjectB = contactSet.ObjectB;
            IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
            ConvexShape      shapeB           = (ConvexShape)geometricObjectB.Shape;
            Vector3          scaleB           = geometricObjectB.Scale;
            Pose             poseB            = geometricObjectB.Pose;

            // Cache inverted rotations.
            var orientationAInverse = poseA.Orientation.Transposed;
            var orientationBInverse = poseB.Orientation.Transposed;

            Vector3 n   = -v0; // Shoot from v0 to the origin.
            Vector3 v1A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
            Vector3 v1B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
            Vector3 v1  = v1B - v1A;

            // Separating axis test:
            if (Vector3.Dot(v1, n) < 0)
            {
                // TODO: We could cache the separating axis n in ContactSet for future collision checks.
                //       Also in the separating axis tests below.
                return(Vector3.Zero);
            }

            // Second support direction = perpendicular to plane of origin, v0 and v1.
            n = Vector3.Cross(v1, v0);

            // If n is a zero vector, then origin, v0 and v1 are on a line with the origin inside the support plane.
            if (n.IsNumericallyZero)
            {
                // Contact found.
                contactSet.HaveContact = true;
                if (type == CollisionQueryType.Boolean)
                {
                    return(Vector3.Zero);
                }

                // Compute contact information.
                // (v0 is an inner point. v1 is a support point on the CSO. => The contact normal is -v1.
                // However, v1 could be close to the origin. To avoid numerical
                // problems we use v0 - v1, which is the same direction.)
                Vector3 normal = v0 - v1;
                if (!normal.TryNormalize())
                {
                    // This happens for Point vs. flat object when they are on the same position.
                    // Maybe we could even find a better normal.
                    normal = Vector3.UnitY;
                }

                Vector3 position         = (v1A + v1B) / 2;
                float   penetrationDepth = v1.Length;
                Contact contact          = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
                ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);

                return(Vector3.Zero);
            }

            Vector3 v2A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
            Vector3 v2B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
            Vector3 v2  = v2B - v2A;

            // Separating axis test:
            if (Vector3.Dot(v2, n) < 0)
            {
                return(Vector3.Zero);
            }

            // Third support direction = perpendicular to plane of v0, v1 and v2.
            n = Vector3.Cross(v1 - v0, v2 - v0);

            // If the origin is on the negative side of the plane, then reverse the plane direction.
            // n must point into the origin direction and not away...
            if (Vector3.Dot(n, v0) > 0)
            {
                MathHelper.Swap(ref v1, ref v2);
                MathHelper.Swap(ref v1A, ref v2A);
                MathHelper.Swap(ref v1B, ref v2B);
                n = -n;
            }

            if (n.IsNumericallyZero)
            {
                // Degenerate case:
                // Interpretation (HelmutG): v2 is on the line with v0 and v1. I think this can only happen
                // if the CSO is flat and in the plane of (origin, v0, v1).
                // This happens for example in Point vs. Line Segment, or triangle vs. triangle when both
                // triangles are in the same plane.
                // Simply ignore this case (Infinite small/flat objects do not touch).
                return(Vector3.Zero);
            }

            // Search for a valid portal.
            Vector3 v3, v3A, v3B;

            while (true)
            {
                iterationCount++;

                // Abort if we cannot find a valid portal.
                if (iterationCount > iterationLimit)
                {
                    return(Vector3.Zero);
                }

                // Get next support point.
                //v3A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
                //v3B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
                //v3 = v3B - v3A;

                // ----- Optimized version:
                Vector3 supportDirectionA;
                supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z);
                supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z);
                supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z);
                Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
                v3A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X;
                v3A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y;
                v3A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z;
                Vector3 supportDirectionB;
                supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z;
                supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z;
                supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z;
                Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
                v3B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X;
                v3B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y;
                v3B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z;
                v3    = v3B - v3A;

                // Separating axis test:
                //if (Vector3.Dot(v3, n) < 0)
                if (v3.X * n.X + v3.Y * n.Y + v3.Z * n.Z < 0)
                {
                    return(Vector3.Zero);
                }

                // v0, v1, v2, v3 form a tetrahedron.
                // v0 is an inner point of the CSO and v1, v2, v3 are support points.
                // v1, v2, v3 should form a valid portal.

                // If origin is outside the plane of v0, v1, v3 then the portal is invalid and we choose a new n.
                //if (Vector3.Dot(Vector3.Cross(v1, v3), v0) < 0) // ORIENT3D test, see Ericson: "Real-Time Collision Detection"
                if ((v1.Y * v3.Z - v1.Z * v3.Y) * v0.X
                    + (v1.Z * v3.X - v1.X * v3.Z) * v0.Y
                    + (v1.X * v3.Y - v1.Y * v3.X) * v0.Z < 0)
                {
                    v2  = v3; // Get rid of v2. A new v3 will be chosen in the next iteration.
                    v2A = v3A;
                    v2B = v3B;
                    //n = Vector3.Cross(v1 - v0, v3 - v0);
                    // ----- Optimized version:
                    Vector3 v1MinusV0;
                    v1MinusV0.X = v1.X - v0.X;
                    v1MinusV0.Y = v1.Y - v0.Y;
                    v1MinusV0.Z = v1.Z - v0.Z;
                    Vector3 v3MinusV0;
                    v3MinusV0.X = v3.X - v0.X;
                    v3MinusV0.Y = v3.Y - v0.Y;
                    v3MinusV0.Z = v3.Z - v0.Z;
                    n.X         = v1MinusV0.Y * v3MinusV0.Z - v1MinusV0.Z * v3MinusV0.Y;
                    n.Y         = v1MinusV0.Z * v3MinusV0.X - v1MinusV0.X * v3MinusV0.Z;
                    n.Z         = v1MinusV0.X * v3MinusV0.Y - v1MinusV0.Y * v3MinusV0.X;
                    continue;
                }

                // If origin is outside the plane of v0, v2, v3 then the portal is invalid and we choose a new n.
                //if (Vector3.Dot(Vector3.Cross(v3, v2), v0) < 0)
                if ((v3.Y * v2.Z - v3.Z * v2.Y) * v0.X
                    + (v3.Z * v2.X - v3.X * v2.Z) * v0.Y
                    + (v3.X * v2.Y - v3.Y * v2.X) * v0.Z < 0)
                {
                    v1  = v3; // Get rid of v1. A new v3 will be chosen in the next iteration.
                    v1A = v3A;
                    v1B = v3B;
                    //n = Vector3.Cross(v3 - v0, v2 - v0);
                    // ----- Optimized version:
                    Vector3 v3MinusV0;
                    v3MinusV0.X = v3.X - v0.X;
                    v3MinusV0.Y = v3.Y - v0.Y;
                    v3MinusV0.Z = v3.Z - v0.Z;
                    Vector3 v2MinusV0;
                    v2MinusV0.X = v2.X - v0.X;
                    v2MinusV0.Y = v2.Y - v0.Y;
                    v2MinusV0.Z = v2.Z - v0.Z;
                    n.X         = v3MinusV0.Y * v2MinusV0.Z - v3MinusV0.Z * v2MinusV0.Y;
                    n.Y         = v3MinusV0.Z * v2MinusV0.X - v3MinusV0.X * v2MinusV0.Z;
                    n.Z         = v3MinusV0.X * v2MinusV0.Y - v3MinusV0.Y * v2MinusV0.X;
                    continue;
                }

                // If come to here, then we have found a valid portal to begin with.
                // (We have a tetrahedron that contains the ray (v0 to origin)).
                break;
            }

            // Refine the portal
            while (true)
            {
                iterationCount++;

                // Store old n. Numerical inaccuracy can lead to endless loops where n is constant.
                Vector3 oldN = n;

                // Compute outward pointing normal of the portal
                //n = Vector3.Cross(v2 - v1, v3 - v1);
                Vector3 v2MinusV1;
                v2MinusV1.X = v2.X - v1.X;
                v2MinusV1.Y = v2.Y - v1.Y;
                v2MinusV1.Z = v2.Z - v1.Z;
                Vector3 v3MinusV1;
                v3MinusV1.X = v3.X - v1.X;
                v3MinusV1.Y = v3.Y - v1.Y;
                v3MinusV1.Z = v3.Z - v1.Z;
                n.X         = v2MinusV1.Y * v3MinusV1.Z - v2MinusV1.Z * v3MinusV1.Y;
                n.Y         = v2MinusV1.Z * v3MinusV1.X - v2MinusV1.X * v3MinusV1.Z;
                n.Z         = v2MinusV1.X * v3MinusV1.Y - v2MinusV1.Y * v3MinusV1.X;

                //if (!n.TryNormalize())
                // ----- Optimized version:
                float nLengthSquared = n.LengthSquared();
                if (nLengthSquared < Numeric.EpsilonFSquared)
                {
                    // The portal is degenerate (some vertices of v1, v2, v3 are identical).
                    // This can happen for coplanar shapes, e.g. long thin triangles in the
                    // same plane. The portal (v1, v2, v3) is a line segment.
                    // This might be a contact or not. We use the GJK as a fallback to check this case.

                    if (_gjk == null)
                    {
                        _gjk = new Gjk(CollisionDetection);
                    }

                    _gjk.ComputeCollision(contactSet, CollisionQueryType.Boolean);
                    if (contactSet.HaveContact == false)
                    {
                        return(Vector3.Zero);
                    }

                    // GJK reports a contact - but it cannot compute contact positions.
                    // We use the best point on the current portal as the contact point.

                    // Find the point closest to the origin.
                    float u, v, w;
                    GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w);
                    Vector3 vClosest = u * v1 + v * v2 + w * v3;

                    // We have not found a separating axis so far. --> Contact.
                    contactSet.HaveContact = true;
                    if (type == CollisionQueryType.Boolean)
                    {
                        return(Vector3.Zero);
                    }

                    // The points on the objects have the same barycentric coordinates.
                    Vector3 pointOnA = u * v1A + v * v2A + w * v3A;
                    Vector3 pointOnB = u * v1B + v * v2B + w * v3B;

                    Vector3 normal = pointOnA - pointOnB;
                    if (!normal.TryNormalize())
                    {
                        if (contactSet.IsPreferredNormalAvailable)
                        {
                            normal = contactSet.PreferredNormal;
                        }
                        else
                        {
                            normal = Vector3.UnitY;
                        }
                    }

                    Vector3 position         = (pointOnA + pointOnB) / 2;
                    float   penetrationDepth = vClosest.Length;
                    Contact contact          = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
                    ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);

                    return(Vector3.Zero);
                }

                // ----- Optimized version: Rest of n.TryNormalize():
                float nLength = (float)Math.Sqrt(nLengthSquared);
                float scale   = 1.0f / nLength;
                n.X *= scale;
                n.Y *= scale;
                n.Z *= scale;

                // Separating axis test:
                // Testing > instead of >= is important otherwise coplanar triangles may report false contacts
                // because the portal is in the same plane as the origin.
                if (!contactSet.HaveContact &&
                    v1.X * n.X + v1.Y * n.Y + v1.Z * n.Z > 0) // Optimized version of && Vector3.Dot(v1, n) > 0)
                {
                    // Portal points aways from origin --> Origin is in the tetrahedron.
                    contactSet.HaveContact = true;
                    if (type == CollisionQueryType.Boolean)
                    {
                        return(Vector3.Zero);
                    }
                }

                // Find new support point.
                //Vector3 v4A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
                //Vector3 v4B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
                //Vector3 v4 = v4B - v4A;

                // ----- Optimized version:
                Vector3 supportDirectionA;
                supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z);
                supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z);
                supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z);
                Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
                Vector3 v4A;
                v4A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X;
                v4A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y;
                v4A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z;
                Vector3 supportDirectionB;
                supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z;
                supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z;
                supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z;
                Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
                Vector3 v4B;
                v4B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X;
                v4B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y;
                v4B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z;
                Vector3 v4 = v4B - v4A;

                // Separating axis test:
                if (!contactSet.HaveContact &&                // <--- New (see below).
                    v4.X * n.X + v4.Y * n.Y + v4.Z * n.Z < 0) // Optimized version of && Vector3.Dot(v4, n) < 0)
                {
                    // Following assert can fail. For example if the above dot product returns -0.000000001
                    // for nearly perfectly touching objects. Therefore I have added the condition
                    // hit == false to the condition.
                    return(Vector3.Zero);
                }

                // Test if we have refined more than the collision epsilon.
                // Condition 1: Project the point difference v4-v3 onto normal n and check whether we have
                // improved in this direction.
                // Condition 2: If n has not changed, then we couldn't improve anymore. This is caused
                // by numerical problems, e.g. when a large object (>10000) is checked.
                //if (Vector3.Dot(v4 - v3, n) <= CollisionDetection.Epsilon
                // ----- Optimized version:
                if ((v4.X - v3.X) * n.X + (v4.Y - v3.Y) * n.Y + (v4.Z - v3.Z) * n.Z <= CollisionDetection.Epsilon ||
                    Vector3.AreNumericallyEqual(n, oldN) ||
                    iterationCount >= iterationLimit)
                {
                    // We have the final portal.
                    if (!contactSet.HaveContact)
                    {
                        return(Vector3.Zero);
                    }

                    if (type == CollisionQueryType.Boolean)
                    {
                        return(Vector3.Zero);
                    }

                    // Find the point closest to the origin.
                    float u, v, w;
                    GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w);

                    // Note: If u, v or w is 0 or 1, then the point was probably outside portal triangle.
                    // We can use the returned data, but re-running MPR will give us a better contact.

                    Vector3 closest = u * v1 + v * v2 + w * v3;

                    // The points on the objects have the same barycentric coordinates.
                    Vector3 pointOnA = u * v1A + v * v2A + w * v3A;
                    Vector3 pointOnB = u * v1B + v * v2B + w * v3B;

                    // Use difference between points as normal direction, only if it can be normalized.
                    Vector3 normal = pointOnA - pointOnB;
                    if (!normal.TryNormalize())
                    {
                        normal = -n; // Else use the inverted normal of the portal.
                    }
                    Vector3 position         = (pointOnA + pointOnB) / 2;
                    float   penetrationDepth = closest.Length;
                    Contact contact          = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
                    ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);

                    // If real closest point is outside the portal triangle, then one of u, v, w will
                    // be exactly 0 or 1. In this case we should run a new MPR with the portal ray n.
                    if (u == 0 || v == 0 || w == 0 || u == 1 || v == 1 || w == 1)
                    {
                        return(n);
                    }

                    return(Vector3.Zero);
                }

                // Now we have a new point v4 and have to make a new portal by eliminating v1, v2 or v3.
                // The possible new tetrahedron faces are: (v0, v1, v4), (v0, v4, v2), (v0, v4, v3)
                // We don't know the orientation yet.
                // Test with the ORIENT3D test.
                //Vector3 cross = Vector3.Cross(v4, v0);
                // ----- Optimized version:
                Vector3 cross;
                cross.X = v4.Y * v0.Z - v4.Z * v0.Y;
                cross.Y = v4.Z * v0.X - v4.X * v0.Z;
                cross.Z = v4.X * v0.Y - v4.Y * v0.X;

                //if (Vector3.Dot(v1, cross) > 0)
                if (v1.X * cross.X + v1.Y * cross.Y + v1.Z * cross.Z > 0)
                {
                    // Eliminate v3 or v1.
                    //if (Vector3.Dot(v2, cross) > 0)
                    if (v2.X * cross.X + v2.Y * cross.Y + v2.Z * cross.Z > 0)
                    {
                        v1  = v4;
                        v1A = v4A;
                        v1B = v4B;
                    }
                    else
                    {
                        v3  = v4;
                        v3A = v4A;
                        v3B = v4B;
                    }
                }
                else
                {
                    // Eliminate v1 or v2.
                    //if (Vector3.Dot(v3, cross) > 0)
                    if (v3.X * cross.X + v3.Y * cross.Y + v3.Z * cross.Z > 0)
                    {
                        v2  = v4;
                        v2A = v4A;
                        v2B = v4B;
                    }
                    else
                    {
                        v1  = v4;
                        v1A = v4A;
                        v1B = v4B;
                    }
                }
            }
        }
Ejemplo n.º 54
0
            public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
            {
                contactSet.Clear();
                contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(1, 2, 3), Vector3F.UnitZ, 1, false));

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

                contactSet.HaveContact = true;
            }
        // Returns true if a contact was added.
        private bool AddContact(ContactSet contactSet,
                                bool swapped,
                                CollisionQueryType type,
                                ref Ray rayWorld,         // The ray in world space.
                                ref Ray rayInField,       // The ray in the scaled height field space.
                                ref Triangle triangle,    // The unscaled triangle in the mesh space.
                                int triangleIndex,
                                ref Pose trianglePose,
                                ref Vector3F triangleScale)
        {
            // This code is from GeometryHelper_Triangles.cs. Sync changes!

            Vector3F v0 = triangle.Vertex0 * triangleScale;
            Vector3F v1 = triangle.Vertex1 * triangleScale;
            Vector3F v2 = triangle.Vertex2 * triangleScale;

            Vector3F d1 = (v1 - v0);
            Vector3F d2 = (v2 - v0);
            Vector3F n  = Vector3F.Cross(d1, d2);

            // Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments".
            float ε = n.Length * Numeric.EpsilonFSquared;

            Vector3F r = rayInField.Direction * rayInField.Length;

            float δ = -Vector3F.Dot(r, n);

            // Degenerate triangle --> No hit.
            if (ε == 0.0f || Numeric.IsZero(δ, ε))
            {
                return(false);
            }

            Vector3F triangleToRayOrigin = rayInField.Origin - v0;
            float    λ = Vector3F.Dot(triangleToRayOrigin, n) / δ;

            if (λ < 0 || λ > 1)
            {
                return(false);
            }

            // The ray hit the triangle plane.
            Vector3F u  = Vector3F.Cross(triangleToRayOrigin, r);
            float    μ1 = Vector3F.Dot(d2, u) / δ;
            float    μ2 = Vector3F.Dot(-d1, u) / δ;

            if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε)
            {
                // Hit!
                contactSet.HaveContact = true;

                if (type == CollisionQueryType.Boolean)
                {
                    return(false);
                }

                if (δ < 0)
                {
                    return(false); // Shooting into the back of a one-sided triangle - no contact.
                }
                float penetrationDepth = λ * rayInField.Length;

                // Create contact info.
                Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
                n = trianglePose.ToWorldDirection(n);

                Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above.");
                n.Normalize();

                if (δ < 0)
                {
                    n = -n;
                }

                if (swapped)
                {
                    n = -n;
                }

                Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true);

                if (swapped)
                {
                    contact.FeatureB = triangleIndex;
                }
                else
                {
                    contact.FeatureA = triangleIndex;
                }

                Debug.Assert(
                    contactSet.ObjectA.GeometricObject.Shape is RayShape && contact.FeatureA == -1 ||
                    contactSet.ObjectB.GeometricObject.Shape is RayShape && contact.FeatureB == -1,
                    "RayHeightFieldAlgorithm has set the wrong feature property.");

                ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
                return(true);
            }

            return(false);
        }
Ejemplo n.º 56
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            if (type == CollisionQueryType.ClosestPoints)
              {
            // Just use normal height field shape algorithm.
            _heightFieldAlgorithm.ComputeCollision(contactSet, type);
            return;
              }

              Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");

              // HeightField = A, Ray = B
              IGeometricObject heightFieldObject = contactSet.ObjectA.GeometricObject;
              IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;

              // Object A should be the height field, swap objects if necessary.
              bool swapped = (heightFieldObject.Shape is RayShape);
              if (swapped)
            MathHelper.Swap(ref rayObject, ref heightFieldObject);

              RayShape rayShape = rayObject.Shape as RayShape;
              HeightField heightField = heightFieldObject.Shape as HeightField;

              // Check if shapes are correct.
              if (rayShape == null || heightField == null)
            throw new ArgumentException("The contact set must contain a ray and a height field.", "contactSet");

              // Assume no contact.
              contactSet.HaveContact = false;

              // Get transformations.
              Vector3F rayScale = rayObject.Scale;
              Pose rayPose = rayObject.Pose;
              Vector3F heightFieldScale = heightFieldObject.Scale;
              Pose heightFieldPose = heightFieldObject.Pose;

              // We do not support negative scaling. It is not clear what should happen when y is
              // scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
              if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0)
            throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");

              // Ray in world space.
              Ray rayWorld = new Ray(rayShape);
              rayWorld.Scale(ref rayScale);
              rayWorld.ToWorld(ref rayPose);

              // Ray in local scaled space of the height field.
              Ray rayScaled = rayWorld;
              rayScaled.ToLocal(ref heightFieldPose);

              // Ray in local unscaled space of the mesh.
              Ray rayUnscaled = rayScaled;
              var inverseCompositeScale = Vector3F.One / heightFieldScale;
              rayUnscaled.Scale(ref inverseCompositeScale);

              // Get height field and basic info.
              int arrayLengthX = heightField.NumberOfSamplesX;
              int arrayLengthZ = heightField.NumberOfSamplesZ;
              int numberOfCellsX = arrayLengthX - 1;
              int numberOfCellsZ = arrayLengthZ - 1;
              Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
              float cellWidthX = heightField.WidthX / numberOfCellsX; // Unscaled!
              float cellWidthZ = heightField.WidthZ / numberOfCellsZ; // Unscaled!

              // We use a 2D-DDA traversal of the height field cells. In other words: Look at it from
              // above. The height field is our screen and we will select the cells as if we draw
              // a pixel line. This could be made more efficient when we do not recompute values and
              // reuse values and make incremental steps Bresenham-style.
              // See GeometryHelper_Casts.cs method HaveContact(Aabb, ray) for explanation of the
              // ray parameter formula.

              var rayUnscaledDirectionInverse = new Vector3F(
            1 / rayUnscaled.Direction.X,
            1 / rayUnscaled.Direction.Y,
            1 / rayUnscaled.Direction.Z);

              // The position where the ray enters the current cell.
              var cellEnter = rayUnscaled.Origin; // Unscaled!!!

              var originX = heightField.OriginX;
              var originZ = heightField.OriginZ;

              // ----- Find first cell.
              int indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; // (int)(...) does not return the desired result for negative values!
              if (indexX < 0)
              {
            if (rayUnscaled.Direction.X <= 0)
              return;

            float parameter = (originX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
            if (parameter > rayUnscaled.Length)
              return;      // The ray does not reach the height field.

            cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
            indexX = 0;
              }
              else if (indexX >= numberOfCellsX)
              {
            if (rayUnscaled.Direction.X >= 0)
              return;

            float parameter = (originX + heightField.WidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
            if (parameter > rayUnscaled.Length)
              return;      // The ray does not reach the height field.

            cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
            indexX = numberOfCellsX - 1;
              }

              int indexZ = (cellEnter.Z >= originZ) ? (int)((cellEnter.Z - originZ) / cellWidthZ) : -1;
              if (indexZ < 0)
              {
            if (rayUnscaled.Direction.Z <= 0)
              return;

            float parameter = (originZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
            if (parameter > rayUnscaled.Length)
              return;      // The ray does not reach the next height field.

            cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
            // We also have to correct the indexX!
            indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
            indexZ = 0;
              }
              else if (indexZ >= numberOfCellsZ)
              {
            if (rayUnscaled.Direction.Z >= 0)
              return;

            float parameter = (originZ + heightField.WidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
            if (parameter > rayUnscaled.Length)
              return;      // The ray does not reach the next height field.

            cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction;
            indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1;
            indexZ = numberOfCellsZ - 1;
              }

              if (indexX < 0 || indexX >= numberOfCellsX || indexZ < 0 || indexZ >= numberOfCellsZ)
            return;

              while (true)
              {
            // ----- Get triangles of current cell.
            var triangle0 = heightField.GetTriangle(indexX, indexZ, false);
            var triangle1 = heightField.GetTriangle(indexX, indexZ, true);

            // Index of first triangle.
            var triangleIndex = (indexZ * numberOfCellsX + indexX) * 2;

            float xRelative = (cellEnter.X - originX) / cellWidthX - indexX;
            float zRelative = (cellEnter.Z - originZ) / cellWidthZ - indexZ;
            bool enterSecondTriangle = (xRelative + zRelative) > 1;  // The diagonal is where xRel + zRel == 1.

            // ----- Find cell exit and move indices to next cell.
            // The position where the ray leaves the current cell.
            Vector3F cellExit;
            float nextXParameter = float.PositiveInfinity;
            if (rayUnscaled.Direction.X > 0)
              nextXParameter = (originX + (indexX + 1) * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;
            else if (rayUnscaled.Direction.X < 0)
              nextXParameter = (originX + indexX * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X;

            float nextZParameter = float.PositiveInfinity;
            if (rayUnscaled.Direction.Z > 0)
              nextZParameter = (originZ + (indexZ + 1) * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;
            else if (rayUnscaled.Direction.Z < 0)
              nextZParameter = (originZ + indexZ * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z;

            bool isLastCell = false;
            if (nextXParameter < nextZParameter)
            {
              if (rayUnscaled.Direction.X > 0)
              {
            indexX++;
            if (indexX >= numberOfCellsX) // Abort if we have left the height field.
              isLastCell = true;
              }
              else
              {
            indexX--;
            if (indexX < 0)
              isLastCell = true;
              }

              if (nextXParameter > rayUnscaled.Length)
              {
            isLastCell = true;  // The ray does not reach the next cell.
            nextXParameter = rayUnscaled.Length;
              }

              cellExit = rayUnscaled.Origin + nextXParameter * rayUnscaled.Direction;
            }
            else
            {
              if (rayUnscaled.Direction.Z > 0)
              {
            indexZ++;
            if (indexZ >= numberOfCellsZ)
              isLastCell = true;
              }
              else
              {
            indexZ--;
            if (indexZ < 0)
              isLastCell = true;
              }

              if (nextZParameter > rayUnscaled.Length)
              {
            isLastCell = true;
            nextZParameter = rayUnscaled.Length;
              }

              cellExit = rayUnscaled.Origin + nextZParameter * rayUnscaled.Direction;
            }

            // ----- We can skip cell if cell AABB is below the ray.
            var rayMinY = Math.Min(cellEnter.Y, cellExit.Y) - CollisionDetection.Epsilon;  // Apply to avoid missing collisions when ray hits a cell border.

            // The ray is above if no height field height is higher the ray height.
            // (This check handles NaN height values (holes) correctly.)
            bool rayIsAbove = !(triangle0.Vertex0.Y >= rayMinY
                            || triangle0.Vertex1.Y >= rayMinY
                            || triangle0.Vertex2.Y >= rayMinY
                            || triangle1.Vertex1.Y >= rayMinY);   // Vertex1 of triangle1 is the fourth quad vertex!

            // ----- Test ray against the 2 triangles of the cell.
            bool triangle0IsHole = false;
            bool triangle1IsHole = false;
            if (!rayIsAbove)
            {
              // Abort if a height value is NaN (hole).
              triangle0IsHole = Numeric.IsNaN(triangle0.Vertex0.Y * triangle0.Vertex1.Y * triangle0.Vertex2.Y);
              triangle1IsHole = Numeric.IsNaN(triangle1.Vertex0.Y * triangle1.Vertex1.Y * triangle1.Vertex2.Y);

              bool contactAdded = false;
              if (enterSecondTriangle)
              {
            // Test second triangle first.
            if (!triangle1IsHole)
              contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
            if (!contactAdded && !triangle0IsHole)
              contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
              }
              else
              {
            // Test first triangle first.
            if (!triangle0IsHole)
              contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale);
            if (!contactAdded && !triangle1IsHole)
              contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale);
              }

              if (contactAdded)
            return;

              // We have contact and stop for boolean queries.
              if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
            return;
            }

            // ----- Return simplified contact if cellEnter is below the cell.
            if (!rayIsAbove)
            {
              if (!enterSecondTriangle && !triangle0IsHole && GeometryHelper.IsInFront(triangle0, cellEnter) < 0
              || enterSecondTriangle && !triangle1IsHole && GeometryHelper.IsInFront(triangle1, cellEnter) < 0)
              {
            contactSet.HaveContact = true;

            if (type == CollisionQueryType.Boolean)
              return;

            var position = heightFieldPose.ToWorldPosition(cellEnter * heightFieldScale);
            var normal = heightFieldPose.ToWorldDirection(Vector3F.UnitY);
            if (swapped)
              normal = -normal;

            float penetrationDepth = (position - rayWorld.Origin).Length;
            Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true);
            ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
            return;
              }
            }

            // ----- Move to next cell.
            if (isLastCell)
              return;

            cellEnter = cellExit;
              }
        }
Ejemplo n.º 57
0
        public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
        {
            // Object A should be the ray.
            // Object B should be the triangle.
            IGeometricObject rayObject      = contactSet.ObjectA.GeometricObject;
            IGeometricObject triangleObject = contactSet.ObjectB.GeometricObject;

            // Swap if necessary.
            bool swapped = (triangleObject.Shape is RayShape);

            if (swapped)
            {
                MathHelper.Swap(ref rayObject, ref triangleObject);
            }

            RayShape      rayShape      = rayObject.Shape as RayShape;
            TriangleShape triangleShape = triangleObject.Shape as TriangleShape;

            // Check if shapes are correct.
            if (rayShape == null || triangleShape == null)
            {
                throw new ArgumentException("The contact set must contain a ray and a triangle.", "contactSet");
            }

            // See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", pp. 84.
            // Note: All computations are done in triangle local space.

            // Get transformations.
            Vector3F rayScale      = rayObject.Scale;
            Vector3F triangleScale = triangleObject.Scale;
            Pose     rayPose       = rayObject.Pose;
            Pose     trianglePose  = triangleObject.Pose;

            // Scale triangle.
            Vector3F v0 = triangleShape.Vertex0 * triangleScale;
            Vector3F v1 = triangleShape.Vertex1 * triangleScale;
            Vector3F v2 = triangleShape.Vertex2 * triangleScale;

            // Scale ray and transform ray to local space of triangle.
            Ray rayWorld = new Ray(rayShape);

            rayWorld.Scale(ref rayScale);  // Scale ray.
            rayWorld.ToWorld(ref rayPose); // Transform to world space.
            Ray ray = rayWorld;

            ray.ToLocal(ref trianglePose); // Transform to local space of triangle.

            Vector3F d1 = (v1 - v0);
            Vector3F d2 = (v2 - v0);
            Vector3F n  = Vector3F.Cross(d1, d2);

            // Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments".
            float ε = n.Length * Numeric.EpsilonFSquared;

            Vector3F r = ray.Direction * ray.Length;

            float δ = -Vector3F.Dot(r, n);

            if (ε == 0.0f || Numeric.IsZero(δ, ε))
            {
                // The triangle is degenerate or the ray is parallel to triangle.
                if (type == CollisionQueryType.Contacts || type == CollisionQueryType.Boolean)
                {
                    contactSet.HaveContact = false;
                }
                else if (type == CollisionQueryType.ClosestPoints)
                {
                    GetClosestPoints(contactSet, rayWorld.Origin);
                }

                return;
            }

            Vector3F triangleToRayOrigin = ray.Origin - v0;
            float    λ = Vector3F.Dot(triangleToRayOrigin, n) / δ;

            // Assume no contact.
            contactSet.HaveContact = false;

            if (λ < 0 || λ > 1)
            {
                // The ray does not hit.
                if (type == CollisionQueryType.ClosestPoints)
                {
                    GetClosestPoints(contactSet, rayWorld.Origin);
                }
            }
            else
            {
                Vector3F u  = Vector3F.Cross(triangleToRayOrigin, r);
                float    μ1 = Vector3F.Dot(d2, u) / δ;
                float    μ2 = Vector3F.Dot(-d1, u) / δ;
                if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε)
                {
                    // Hit!
                    contactSet.HaveContact = true;

                    if (type == CollisionQueryType.Boolean)
                    {
                        return;
                    }

                    float penetrationDepth = λ * ray.Length;

                    // Create contact info.
                    Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
                    n = trianglePose.ToWorldDirection(n);

                    Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above.");
                    n.Normalize();

                    if (δ > 0)
                    {
                        n = -n;
                    }

                    if (swapped)
                    {
                        n = -n;
                    }

                    Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true);
                    ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
                }
                else
                {
                    // No Hit!
                    if (type == CollisionQueryType.ClosestPoints)
                    {
                        GetClosestPoints(contactSet, rayWorld.Origin);
                    }
                }
            }
        }
Ejemplo n.º 58
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
        }
Ejemplo n.º 59
0
        public override void Update(GameTime gameTime)
        {
            var debugRenderer = GraphicsScreen.DebugRenderer;

            debugRenderer.Clear();

            // Draw coordinate cross at world origin.
            debugRenderer.DrawAxes(Pose.Identity, 10, false);

            // Draw objects.
            debugRenderer.DrawObject(_objectA.GeometricObject, GraphicsHelper.GetUniqueColor(_objectA), _drawWireframe, false);
            debugRenderer.DrawObject(_objectB.GeometricObject, GraphicsHelper.GetUniqueColor(_objectB), _drawWireframe, false);

            // Draw contact info.
            debugRenderer.DrawContacts(_contactSet, 0.1f, null, true);

            // Toggle wireframe rendering.
            if (InputService.IsPressed(Keys.Space, true))
            {
                _drawWireframe = !_drawWireframe;
            }

            // Change background color if we have a contact.
            GraphicsScreen.BackgroundColor = (_contactSet != null && _contactSet.HaveContact)
        ? new Color(220, 200, 200, 255)
        : new Color(200, 220, 200, 255);

            // Move one object with keyboard NumPad.
            var translation = new Vector3F();

            if (InputService.IsDown(Keys.NumPad4))
            {
                translation.X -= 1;
            }
            if (InputService.IsDown(Keys.NumPad6))
            {
                translation.X += 1;
            }
            if (InputService.IsDown(Keys.NumPad8))
            {
                translation.Y += 1;
            }
            if (InputService.IsDown(Keys.NumPad5))
            {
                translation.Y -= 1;
            }
            if (InputService.IsDown(Keys.NumPad7))
            {
                translation.Z -= 1;
            }
            if (InputService.IsDown(Keys.NumPad9))
            {
                translation.Z += 1;
            }

            if (!translation.IsNumericallyZero)
            {
                var go = (GeometricObject)_objectA.GeometricObject;

                float scale = go.Aabb.Extent.Length * 0.1f;
                translation *= scale * (float)gameTime.ElapsedGameTime.TotalSeconds;

                go.Pose = new Pose(go.Pose.Position + translation, go.Pose.Orientation);

                if (_contactSet != null)
                {
                    _collisionDetection.UpdateContacts(_contactSet, 0.001f);
                }
                else
                {
                    _contactSet = _collisionDetection.GetContacts(_objectA, _objectB);
                }
            }
        }
Ejemplo n.º 60
0
        public CollisionAlgorithm this[ContactSet pair]
        {
            get
              {
            if (pair == null)
              throw new ArgumentNullException("pair");

            Debug.Assert(pair.ObjectA != null, "ContactSet needs to ensure that ObjectA is not null.");
            Debug.Assert(pair.ObjectB != null, "ContactSet needs to ensure that ObjectB is not null.");
            Debug.Assert(pair.ObjectA.GeometricObject != null, "CollisionObject needs to ensure that GeometricObject is not null.");
            Debug.Assert(pair.ObjectB.GeometricObject != null, "CollisionObject needs to ensure that GeometricObject is not null.");
            Debug.Assert(pair.ObjectA.GeometricObject.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
            Debug.Assert(pair.ObjectB.GeometricObject.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");

            return this[pair.ObjectA.GeometricObject.Shape.GetType(), pair.ObjectB.GeometricObject.Shape.GetType()];
              }
              set
              {
            if (pair == null)
              throw new ArgumentNullException("pair");

            Debug.Assert(pair.ObjectA != null, "ContactSet needs to ensure that ObjectA is not null.");
            Debug.Assert(pair.ObjectB != null, "ContactSet needs to ensure that ObjectB is not null.");
            Debug.Assert(pair.ObjectA.GeometricObject != null, "CollisionObject needs to ensure that GeometricObject is not null.");
            Debug.Assert(pair.ObjectB.GeometricObject != null, "CollisionObject needs to ensure that GeometricObject is not null.");
            Debug.Assert(pair.ObjectA.GeometricObject.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
            Debug.Assert(pair.ObjectB.GeometricObject.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");

            this[pair.ObjectA.GeometricObject.Shape.GetType(), pair.ObjectB.GeometricObject.Shape.GetType()] = value;
              }
        }