Пример #1
0
        public void ToWorld()
        {
            Vector3F point0 = new Vector3F(10, 20, -40);
              Vector3F point1 = new Vector3F(-22, 34, 45);
              Line line = new Line(point0, (point1 - point0).Normalized);

              Pose pose = new Pose(new Vector3F(-5, 100, -20), Matrix33F.CreateRotation(new Vector3F(1, 2, 3), 0.123f));
              point0 = pose.ToWorldPosition(point0);
              point1 = pose.ToWorldPosition(point1);
              line.ToWorld(ref pose);

              Vector3F dummy;
              Assert.IsTrue(GeometryHelper.GetClosestPoint(line, point0, out dummy));
              Assert.IsTrue(GeometryHelper.GetClosestPoint(line, point1, out dummy));
        }
Пример #2
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);
        }