C# Ray.ToWorld方法代码示例

        public void ToWorld()
        {
            Vector3F startPoint = new Vector3F(10, 20, -40);
              Vector3F endPoint = new Vector3F(-22, 34, 45);
              Ray ray = new Ray(startPoint, (endPoint - startPoint).Normalized, (endPoint - startPoint).Length);

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

              Assert.IsTrue(Vector3F.AreNumericallyEqual(startPoint, ray.Origin));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(endPoint, ray.Origin + ray.Direction * ray.Length));
        }

        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;
              }
            }
              }
        }