C# Pose.ToWorldPosition方法代码示例

        /// <summary>
        /// Updates the occluder data.
        /// </summary>
        /// <param name="occluder">The occluder.</param>
        /// <param name="pose">The pose of the <see cref="OccluderNode"/>.</param>
        /// <param name="scale">The scale of the <see cref="OccluderNode"/>.</param>
        public void Update(Occluder occluder, Pose pose, Vector3F scale)
        {
            Debug.Assert(
            Vertices.Length == occluder.Vertices.Length
            && Indices == occluder.Indices,
            "OccluderData does not match.");

              Vector3F[] localVertices = occluder.Vertices;
              if (scale == Vector3F.One)
              {
            for (int i = 0; i < Vertices.Length; i++)
              Vertices[i] = pose.ToWorldPosition(localVertices[i]);
              }
              else
              {
            for (int i = 0; i < Vertices.Length; i++)
              Vertices[i] = pose.ToWorldPosition(scale * localVertices[i]);
              }

              // Update of large occluders could be accelerated by using a parallel for-loop.
              // However, most occluders are small, occluders are already updated in parallel,
              // and static occluders only need to be updated once.
        }

    /// <inheritdoc/>
    public override Aabb GetAabb(Vector3F scale, Pose pose)
    {
      if (scale.X == scale.Y && scale.Y == scale.Z)
      {
        // Uniform scaling.
        float uniformScale = Math.Abs(scale.X);
        float scaledHeight = uniformScale * _height;
        float scaledRadius = uniformScale * _radius;

        float halfHeightWithoutCaps = (scaledHeight / 2 - scaledRadius);

        // Imagine the skeleton of the capsule as a line: 
        //   (0, -halfExtentWithoutCaps, 0) to (0, halfExtent, 0)

        // To create the AABB we rotate these to points and then just add the radius.
        Vector3F p1 = pose.ToWorldPosition(new Vector3F(0, halfHeightWithoutCaps, 0));
        Vector3F p2 = pose.ToWorldPosition(new Vector3F(0, -halfHeightWithoutCaps, 0));
        Vector3F radius = new Vector3F(scaledRadius);
        Vector3F minimum = Vector3F.Min(p1, p2) - radius;
        Vector3F maximum = Vector3F.Max(p1, p2) + radius;
        return new Aabb(minimum, maximum);
      }
      else
      {
        // Non-uniform scaling.
        return base.GetAabb(scale, pose);
      }
    }

        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 void ComputeBoxForCube()
        {
            Vector3F box;
              Pose pose;

              // Cube
              var points = new List<Vector3F>()
              {
            new Vector3F(-1, -1, -1),
            new Vector3F(-1, -1, 1),
            new Vector3F(-1, 1, -1),
            new Vector3F(-1, 1, 1),
            new Vector3F(1, -1, -1),
            new Vector3F(1, -1, 1),
            new Vector3F(1, 1, -1),
            new Vector3F(1, 1, 1),
              };

              // Translate and rotate cube.
              Pose cubePose = new Pose(new Vector3F(10, 2, 3), RandomHelper.Random.NextQuaternionF());
              for (int i = 0; i < points.Count; i++)
            points[i] = cubePose.ToWorldPosition(points[i]);

              GeometryHelper.ComputeBoundingBox(points, out box, out pose);
              Assert.IsTrue(Numeric.AreEqual(box.X, 2, 0.2f));
              Assert.IsTrue(Numeric.AreEqual(box.Y, 2, 0.2f));
              Assert.IsTrue(Numeric.AreEqual(box.Z, 2, 0.2f));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(10, 2, 3), pose.Position));
        }

        public void ComputeBoxForCapsules()
        {
            Vector3F box;
              Pose pose;

              var points = new CapsuleShape(1, 3).GetMesh(0.01f, 4).Vertices;

              Pose cubePose = new Pose(new Vector3F(10, 2, 3), RandomHelper.Random.NextQuaternionF());
              for (int i = 0; i < points.Count; i++)
            points[i] = cubePose.ToWorldPosition(points[i]);

              GeometryHelper.ComputeBoundingBox(points, out box, out pose);
              Assert.IsTrue(Numeric.AreEqual(box.LargestComponent, 3, 0.2f));
              Assert.IsTrue(Numeric.AreEqual(box.SmallestComponent, 2, 0.2f));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(10, 2, 3), pose.Position));
        }

        public void DrawAabb(Aabb aabb, Pose pose, Color color, bool drawOverScene)
        {
            if (!Enabled)
            return;

              Vector3F corner0 = pose.ToWorldPosition(new Vector3F(aabb.Minimum.X, aabb.Minimum.Y, aabb.Maximum.Z));
              Vector3F corner1 = pose.ToWorldPosition(new Vector3F(aabb.Maximum.X, aabb.Minimum.Y, aabb.Maximum.Z));
              Vector3F corner2 = pose.ToWorldPosition(aabb.Maximum);
              Vector3F corner3 = pose.ToWorldPosition(new Vector3F(aabb.Minimum.X, aabb.Maximum.Y, aabb.Maximum.Z));
              Vector3F corner4 = pose.ToWorldPosition(aabb.Minimum);
              Vector3F corner5 = pose.ToWorldPosition(new Vector3F(aabb.Maximum.X, aabb.Minimum.Y, aabb.Minimum.Z));
              Vector3F corner6 = pose.ToWorldPosition(new Vector3F(aabb.Maximum.X, aabb.Maximum.Y, aabb.Minimum.Z));
              Vector3F corner7 = pose.ToWorldPosition(new Vector3F(aabb.Minimum.X, aabb.Maximum.Y, aabb.Minimum.Z));

              DrawLine(corner0, corner1, color, drawOverScene);
              DrawLine(corner1, corner2, color, drawOverScene);
              DrawLine(corner2, corner3, color, drawOverScene);
              DrawLine(corner0, corner3, color, drawOverScene);
              DrawLine(corner4, corner5, color, drawOverScene);
              DrawLine(corner5, corner6, color, drawOverScene);
              DrawLine(corner6, corner7, color, drawOverScene);
              DrawLine(corner7, corner4, color, drawOverScene);
              DrawLine(corner0, corner4, color, drawOverScene);
              DrawLine(corner1, corner5, color, drawOverScene);
              DrawLine(corner2, corner6, color, drawOverScene);
              DrawLine(corner3, corner7, color, drawOverScene);
        }

        // Computes the volume of the submerged mesh part and the center of buoyancy.
        private float GetSubmergedVolume(Vector3F scale, Pose pose, TriangleMesh mesh, out Vector3F center)
        {
            center = Vector3F.Zero;

              // Get surface plane in local space.
              Plane planeLocal = new Plane
              {
            Normal = pose.ToLocalDirection(Surface.Normal),
            DistanceFromOrigin = Surface.DistanceFromOrigin - Vector3F.Dot(Surface.Normal, pose.Position),
              };

              const float tinyDepth = -1e-6f;

              // Vertex heights relative to surface plane. Positive = above water.
              int numberOfVertices = mesh.Vertices.Count;

              // Compute depth of each vertex.
              List<float> depths = ResourcePools<float>.Lists.Obtain();

              // Use try-finally block to properly recycle resources from pool.
              try
              {
            int numberOfSubmergedVertices = 0;
            int sampleVertexIndex = 0;
            for (int i = 0; i < numberOfVertices; i++)
            {
              float depth = Vector3F.Dot(planeLocal.Normal, mesh.Vertices[i] * scale) - planeLocal.DistanceFromOrigin;
              if (depth < tinyDepth)
              {
            numberOfSubmergedVertices++;
            sampleVertexIndex = i;
              }

              depths.Add(depth);
            }

            // Abort if no vertex is in water.
            if (numberOfSubmergedVertices == 0)
              return 0;

            // Get the reference point. We project a submerged vertex onto the surface plane.
            Vector3F point = mesh.Vertices[sampleVertexIndex] - depths[sampleVertexIndex] * planeLocal.Normal;

            float volume = 0;

            // Add contribution of each triangle.
            int numberOfTriangles = mesh.NumberOfTriangles;
            for (int i = 0; i < numberOfTriangles; i++)
            {
              // Triangle vertex indices.
              int i0 = mesh.Indices[i * 3 + 0];
              int i1 = mesh.Indices[i * 3 + 1];
              int i2 = mesh.Indices[i * 3 + 2];

              // Vertices and depths.
              Vector3F v0 = mesh.Vertices[i0] * scale;
              float d0 = depths[i0];
              Vector3F v1 = mesh.Vertices[i1] * scale;
              float d1 = depths[i1];
              Vector3F v2 = mesh.Vertices[i2] * scale;
              float d2 = depths[i2];

              if (d0 * d1 < 0)
              {
            // v0 - v1 crosses the surface.
            volume += ClipTriangle(point, v0, v1, v2, d0, d1, d2, ref center);
              }
              else if (d0 * d2 < 0)
              {
            // v0 - v2 crosses the surface.
            volume += ClipTriangle(point, v2, v0, v1, d2, d0, d1, ref center);
              }
              else if (d1 * d2 < 0)
              {
            // v1 - v2 crosses the surface.
            volume += ClipTriangle(point, v1, v2, v0, d1, d2, d0, ref center);
              }
              else if (d0 < 0 || d1 < 0 || d2 < 0)
              {
            // Fully submerged triangle.
            volume += GetSignedTetrahedronVolume(point, v0, v1, v2, ref center);
              }
            }

            // If the volume is near zero or negative (numerical errors), we abort.
            const float tinyVolume = 1e-6f;
            if (volume <= tinyVolume)
            {
              center = Vector3F.Zero;
              return 0;
            }

            // Normalize the center (was weighted by volume).
            center = center / volume;

            // Transform center to world space.
            center = pose.ToWorldPosition(center);

            return volume;
//.........这里部分代码省略.........

 /// <inheritdoc/>
 public override Aabb GetAabb(Vector3F scale, Pose pose)
 {
   // Note: Compute AABB in world space
   Vector3F vertex0 = pose.ToWorldPosition(_vertex0 * scale);
   Vector3F vertex1 = pose.ToWorldPosition(_vertex1 * scale);
   Vector3F vertex2 = pose.ToWorldPosition(_vertex2 * scale);
   Vector3F minimum = Vector3F.Min(vertex0, Vector3F.Min(vertex1, vertex2));
   Vector3F maximum = Vector3F.Max(vertex0, Vector3F.Max(vertex1, vertex2));
   return new Aabb(minimum, maximum);
 }

    /// <inheritdoc/>
    public override Aabb GetAabb(Vector3F scale, Pose pose)
    {
      // Note: Compute AABB in world space.
      Vector3F direction = pose.ToWorldDirection(_direction * scale);
      Vector3F pointOnLine = pose.ToWorldPosition(_pointOnLine * scale);

      // Most of the time the AABB fills the whole space. Only when the line is axis-aligned then
      // the AABB is different.
      Vector3F minimum = new Vector3F(float.NegativeInfinity);
      Vector3F maximum = new Vector3F(float.PositiveInfinity);

      // Using numerical comparison we "clamp" the line into an axis-aligned plane if possible.
      if (Numeric.IsZero(direction.X))
      {
        minimum.X = pointOnLine.X;
        maximum.X = pointOnLine.X;
      }
      if (Numeric.IsZero(direction.Y))
      {
        minimum.Y = pointOnLine.Y;
        maximum.Y = pointOnLine.Y;
      }
      if (Numeric.IsZero(direction.Z))
      {
        minimum.Z = pointOnLine.Z;
        maximum.Z = pointOnLine.Z;
      }

      return new Aabb(minimum, maximum);
    }

 /// <inheritdoc/>
 public override Aabb GetAabb(Vector3F scale, Pose pose)
 {
   // Note: Compute AABB in world space
   Vector3F position = pose.ToWorldPosition(scale * _position);
   return new Aabb(position, position);
 }

 /// <inheritdoc/>
 public override Aabb GetAabb(Vector3F scale, Pose pose)
 {
   Vector3F worldStart = pose.ToWorldPosition(_start * scale);
   Vector3F worldEnd = pose.ToWorldPosition(_end * scale);
   Vector3F minimum = Vector3F.Min(worldStart, worldEnd);
   Vector3F maximum = Vector3F.Max(worldStart, worldEnd);
   return new Aabb(minimum, maximum);
 }

        // Checks a vertex and adds a contact if the vertex touches the plane.
        private void CheckContact(ref Plane planeWorld, Vector3F vertexLocal, ref Pose poseBox, bool swapped, ContactSet contactSet)
        {
            Vector3F vertex = poseBox.ToWorldPosition(vertexLocal);

              float distance = Vector3F.Dot(vertex, planeWorld.Normal);

              float penetrationDepth = planeWorld.DistanceFromOrigin - distance;

              if (penetrationDepth > 0)
              {
            // Position is between support vertex and plane.
            AddContact(ref vertex, ref planeWorld, penetrationDepth, swapped, contactSet, CollisionQueryType.Contacts);
              }
        }

 /// <inheritdoc/>
 public override Aabb GetAabb(Vector3F scale, Pose pose)
 {
   Vector3F scaledOrigin = _origin * scale;
   Vector3F worldStart = pose.ToWorldPosition(scaledOrigin);
   Vector3F worldEnd = pose.ToWorldPosition(scaledOrigin + scale * _direction * _length);
   Vector3F minimum = Vector3F.Min(worldStart, worldEnd);
   Vector3F maximum = Vector3F.Max(worldStart, worldEnd);
   return new Aabb(minimum, maximum);
 }

        public void NegativeScale()
        {
            // What happens to the triangle normal when a negative scale is applied?
              // --> To get the correct normal from the transformed mesh, we have to change
              // the winding order if an odd number of scale components (X, Y, Z) are negative.

              RandomHelper.Random = new Random(1234567);
              for (int i = 0; i < 100; i++)
              {
            var tA = new Triangle();
            tA.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), RandomHelper.Random.NextFloat(-100, 100), RandomHelper.Random.NextFloat(-100, 100));
            tA.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), RandomHelper.Random.NextFloat(-100, 100), RandomHelper.Random.NextFloat(-100, 100));
            tA.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), RandomHelper.Random.NextFloat(-100, 100), RandomHelper.Random.NextFloat(-100, 100));

            // Random scale including negative scale for mirroring.
            var s = new Vector3F(RandomHelper.Random.NextFloat(-2, 2), RandomHelper.Random.NextFloat(-2, 2), RandomHelper.Random.NextFloat(-2, 2));

            // Random pose.
            var p = new Pose(
              new Vector3F(RandomHelper.Random.NextFloat(-100, 100), RandomHelper.Random.NextFloat(-100, 100), RandomHelper.Random.NextFloat(-100, 100)),
              RandomHelper.Random.NextQuaternionF());

            // For the correct triangle normal we have to use the inverse transpose:
            //   (M^-1)^T = 1 / scale
            var n = Vector3F.Cross(tA.Vertex1 - tA.Vertex0, tA.Vertex2 - tA.Vertex0) / s;
            n = p.ToWorldDirection(n);

            if (n.TryNormalize())
            {
              // Lets transform the triangle.
              tA.Vertex0 = p.ToWorldPosition(tA.Vertex0 * s);
              tA.Vertex1 = p.ToWorldPosition(tA.Vertex1 * s);
              tA.Vertex2 = p.ToWorldPosition(tA.Vertex2 * s);

              // Change the winding order, so that we get the same result.
              if (s.X * s.Y * s.Z < 0)
            MathHelper.Swap(ref tA.Vertex0, ref tA.Vertex1);

              bool areEqual = Vector3F.AreNumericallyEqual(n, tA.Normal, 0.001f);
              if (!areEqual)
            Debugger.Break();
              Assert.IsTrue(areEqual);
            }
              }
        }

        /// <summary>
        /// Computes a squared lower bound for the distance between the two oriented boxes.
        /// </summary>
        /// <param name="boxExtentA">The extent (the widths in x, y and z) of the first box.</param>
        /// <param name="poseA">The pose of the first box.</param>
        /// <param name="boxExtentB">The extent (the widths in x, y and z) of the second box.</param>
        /// <param name="poseB">The pose of second box.</param>
        /// <returns>The squared lower bound for the distance between the two oriented boxes.</returns>
        internal static float GetDistanceLowerBoundSquared(Vector3F boxExtentA, Pose poseA, Vector3F boxExtentB, Pose poseB)
        {
            Vector3F aToB = poseB.Position - poseA.Position;
              float distanceSquared = aToB.LengthSquared;
              if (Numeric.IsZero(distanceSquared))
            return 0;

              Vector3F closestPointOnB = poseB.ToWorldPosition(GetSupportPoint(boxExtentB, poseB.ToLocalDirection(-aToB)));
              Vector3F closestPointOnA = poseA.ToWorldPosition(GetSupportPoint(boxExtentA, poseA.ToLocalDirection(aToB)));
              Vector3F closestPointVector = closestPointOnB - closestPointOnA;

              // Use dot product to project closest-point pair vector onto center line.
              float dot = Vector3F.Dot(aToB, closestPointVector);

              // If dot is negative we can have contact.
              if (dot <= 0)
            return 0;

              // Compute rest of vector projection.
              return dot * dot / distanceSquared;
        }