C# Pose.ToWorldDirection方法代码示例

    /// <inheritdoc/>
    public override Aabb GetAabb(Vector3F scale, Pose pose)
    {
      // Uniform scales do not influence the normal direction. Non-uniform scales change the normal
      // direction, but don't make much sense for planes. --> Return an infinite AABB.
      if (scale.X != scale.Y || scale.Y != scale.Z)
        return new Aabb(new Vector3F(float.NegativeInfinity), new Vector3F(float.PositiveInfinity));

      // Note: Compute AABB in world space
      Vector3F normal = pose.ToWorldDirection(_normal);

      // Negative uniform scaling --> invert normal.
      if (scale.X < 0)
        normal = -normal;

      // Apply scaling.
      float scaledDistance = _distanceFromOrigin * scale.X;

      // Most of the time the AABB fills the whole space. Only when the plane is axis-aligned then
      // the AABB is different.
      // Using numerical comparison we "clamp" the plane to an axis-aligned plane if possible.
      if (Vector3F.AreNumericallyEqual(normal, Vector3F.UnitX))
      {
        Vector3F minimum = new Vector3F(float.NegativeInfinity);
        Vector3F maximum = new Vector3F(pose.Position.X + scaledDistance, float.PositiveInfinity, float.PositiveInfinity);
        return new Aabb(minimum, maximum);
      }
      else if (Vector3F.AreNumericallyEqual(normal, Vector3F.UnitY))
      {
        Vector3F minimum = new Vector3F(float.NegativeInfinity);
        Vector3F maximum = new Vector3F(float.PositiveInfinity, pose.Position.Y + scaledDistance, float.PositiveInfinity);
        return new Aabb(minimum, maximum);
      }
      else if (Vector3F.AreNumericallyEqual(normal, Vector3F.UnitZ))
      {
        Vector3F minimum = new Vector3F(float.NegativeInfinity);
        Vector3F maximum = new Vector3F(float.PositiveInfinity, float.PositiveInfinity, pose.Position.Z + scaledDistance);
        return new Aabb(minimum, maximum);
      }
      else if (Vector3F.AreNumericallyEqual(normal, -Vector3F.UnitX))
      {
        Vector3F minimum = new Vector3F(pose.Position.X - scaledDistance, float.NegativeInfinity, float.NegativeInfinity);
        Vector3F maximum = new Vector3F(float.PositiveInfinity);
        return new Aabb(minimum, maximum);
      }
      else if (Vector3F.AreNumericallyEqual(normal, -Vector3F.UnitY))
      {
        Vector3F minimum = new Vector3F(float.NegativeInfinity, pose.Position.Y - scaledDistance, float.NegativeInfinity);
        Vector3F maximum = new Vector3F(float.PositiveInfinity);
        return new Aabb(minimum, maximum);
      }
      else if (Vector3F.AreNumericallyEqual(normal, -Vector3F.UnitZ))
      {
        Vector3F minimum = new Vector3F(float.NegativeInfinity, float.NegativeInfinity, pose.Position.Z - scaledDistance);
        Vector3F maximum = new Vector3F(float.PositiveInfinity);
        return new Aabb(minimum, maximum);
      }
      else
      {
        // Plane is not axis-aligned. --> AABB is infinite
        return new Aabb(new Vector3F(float.NegativeInfinity), new Vector3F(float.PositiveInfinity));
      }
    }

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

        /// <summary>
        /// Draws an arc to visualize a rotation limit about an axis.
        /// </summary>
        /// <param name="constraintToWorld">
        /// A transformation that transforms from constraint anchor space to world space.
        /// </param>
        /// <param name="center">The center of the circle.</param>
        /// <param name="axis">The rotation axis.</param>
        /// <param name="direction">A direction vector (e.g. the direction of a bone).</param>
        /// <param name="minimum">The minimum angle.</param>
        /// <param name="maximum">The maximum angle.</param>
        /// <param name="color">The color.</param>
        private static void DrawArc(Pose constraintToWorld, Vector3F center, Vector3F axis, Vector3F direction, float minimum, float maximum, Color color)
        {
            if (minimum == 0 && maximum == 0)
            return;

              // Line from circle center to start of arc.
              _points[_pointCount] = new VertexPositionColor((Vector3)center, color);
              IncrementPointCount();
              Vector3F previousArcPoint = center + _scale * constraintToWorld.ToWorldDirection(QuaternionF.CreateRotation(axis, minimum).Rotate(direction));
              _points[_pointCount] = new VertexPositionColor((Vector3)previousArcPoint, color);
              IncrementPointCount();

              // Draw arc.
              int numberOfSegments = (int)Math.Max((maximum - minimum) / (ConstantsF.Pi / 24), 1);
              float segmentAngle = (maximum - minimum) / numberOfSegments;
              for (int i = 0; i < numberOfSegments; i++)
              {
            Vector3F arcPoint = center + _scale * constraintToWorld.ToWorldDirection(QuaternionF.CreateRotation(axis, minimum + (i + 1) * segmentAngle).Rotate(direction));

            _points[_pointCount] = new VertexPositionColor((Vector3)previousArcPoint, color);
            IncrementPointCount();
            _points[_pointCount] = new VertexPositionColor((Vector3)arcPoint, color);
            IncrementPointCount();

            previousArcPoint = arcPoint;
              }

              // Line from end of arc to circle center.
              _points[_pointCount] = new VertexPositionColor((Vector3)previousArcPoint, color);
              IncrementPointCount();
              _points[_pointCount] = new VertexPositionColor((Vector3)center, color);
              IncrementPointCount();
        }

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

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

        /// <summary>
        /// Draws 3 axes for a coordinate cross.
        /// </summary>
        /// <param name="pose">The pose (position and orientation).</param>
        /// <param name="size">The size in world space.</param>
        /// <param name="drawOverScene">
        /// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test 
        /// disabled).
        /// </param>
        /// <remarks>
        /// The x-axis is drawn red, the y-axis is drawn green, and the z-axis is drawn blue.
        /// </remarks>
        public void DrawAxes(Pose pose, float size, bool drawOverScene)
        {
            if (!Enabled)
            return;

              DrawArrow(pose.Position, pose.Position + pose.ToWorldDirection(Vector3F.UnitX) * size, Color.Red, drawOverScene);
              DrawArrow(pose.Position, pose.Position + pose.ToWorldDirection(Vector3F.UnitY) * size, Color.Green, drawOverScene);
              DrawArrow(pose.Position, pose.Position + pose.ToWorldDirection(Vector3F.UnitZ) * size, Color.Blue, drawOverScene);
        }

    // Gets neighbor normal and dot product of normals for contact welding.
    private static void GetNeighborNormal(
      int triangleIndex, Triangle triangle, Vector3F contactPositionOnTriangle, Vector3F triangleNormal, 
      TriangleMeshShape triangleMeshShape, Pose poseA, Vector3F scaleA, 
      out Vector3F neighborNormal, out float triangleDotNeighbor)
    {
      // Get barycentric coordinates of contact position.
      float u, v, w;
      // TODO: GetBaryCentricFromPoint computes the triangle normal, which we already know - optimize.
      GeometryHelper.GetBarycentricFromPoint(triangle, contactPositionOnTriangle, out u, out v, out w);

      // Find neighbor triangle normal.
      // If we do not find a neighbor, we assume the neighbor has the same normal.
      neighborNormal = triangleNormal;
      triangleDotNeighbor = float.MaxValue;

      // TODO: Optimize: We could trade memory for performance and store the precomputed triangle normals.

      // If one coordinate is near 0, the contact is near an edge.
      if (u < 0.05f || v < 0.05f || w < 0.05f)
      {
        if (u < 0.05f)
        {
          int neighborIndex = triangleMeshShape.TriangleNeighbors[triangleIndex * 3 + 0];
          if (neighborIndex >= 0)
          {
            Triangle neighbor = triangleMeshShape.Mesh.GetTriangle(neighborIndex);
            var newNeighborNormal = poseA.ToWorldDirection(neighbor.Normal / scaleA);
              // TODO: Optimize: neighbor.Normal normalizes the normal but we denormalize it with 1/scaleA.
            if (newNeighborNormal.TryNormalize())
            {
              float dot = Vector3F.Dot(triangleNormal, newNeighborNormal);
              if (dot < triangleDotNeighbor)
              {
                triangleDotNeighbor = dot;
                neighborNormal = newNeighborNormal;
              }
            }
          }
        }
        if (v < 0.05f)
        {
          int neighborIndex = triangleMeshShape.TriangleNeighbors[triangleIndex * 3 + 1];
          if (neighborIndex >= 0)
          {
            Triangle neighbor = triangleMeshShape.Mesh.GetTriangle(neighborIndex);
            var newNeighborNormal = poseA.ToWorldDirection(neighbor.Normal / scaleA);
            if (newNeighborNormal.TryNormalize())
            {
              float dot = Vector3F.Dot(triangleNormal, newNeighborNormal);
              if (dot < triangleDotNeighbor)
              {
                triangleDotNeighbor = dot;
                neighborNormal = newNeighborNormal;
              }
            }
          }
        }
        if (w < 0.05f)
        {
          int neighborIndex = triangleMeshShape.TriangleNeighbors[triangleIndex * 3 + 2];
          if (neighborIndex >= 0)
          {
            Triangle neighbor = triangleMeshShape.Mesh.GetTriangle(neighborIndex);
            var newNeighborNormal = poseA.ToWorldDirection(neighbor.Normal / scaleA);
            if (newNeighborNormal.TryNormalize())
            {
              float dot = Vector3F.Dot(triangleNormal, newNeighborNormal);
              if (dot < triangleDotNeighbor)
              {
                triangleDotNeighbor = dot;
                neighborNormal = newNeighborNormal;
              }
            }
          }
        }
      }
    }