C# Pose.ToLocalDirection方法代码示例

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

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