C# Vector3F.TryNormalize方法代码示例

        private static void GetEllipseCoefficients(Vector3F normalView, out Vector3F axisMajor, out Vector3F axisMinor, out float radiusMajor, out float radiusMinor)
        {
            axisMinor = new Vector3F(normalView.X, normalView.Y, 0);
              if (!axisMinor.TryNormalize())
            axisMinor = new Vector3F(0, 1, 0);

              Vector3F normalScreen = new Vector3F(0, 0, 1); // The normal vector of the screen.
              axisMajor = Vector3F.Cross(axisMinor, normalScreen);
              radiusMinor = Vector3F.Dot(normalView, normalScreen);
              radiusMajor = 1;
        }

        public static void CreateNormalTexture(GraphicsDevice graphicsDevice, float[] heights,
            int textureWidth, int textureHeight, float cellSize,
            bool useNearestNeighborFilter, ref Texture2D normalTexture)
        {
            if (graphicsDevice == null)
            throw new ArgumentNullException("graphicsDevice");
              if (heights == null)
            throw new ArgumentNullException("heights");

              int w = textureWidth;
              int h = textureHeight;

              if (normalTexture != null)
              {
            if (normalTexture.Width != w)
              throw new ArgumentException("Width of normal texture does not match textureWidth.");
            if (normalTexture.Height != h)
              throw new ArgumentException("Height of normal texture does not match textureHeight.");
            if (normalTexture.Format != SurfaceFormat.Color)
              throw new ArgumentException("Format of normal texture must be Color.");
            if ((w > 1 || h > 1) && normalTexture.LevelCount == 1)
              throw new ArgumentException("Normal texture does not have mipmaps.");
              }
              else
              {
            normalTexture = new Texture2D(graphicsDevice, w, h, true, SurfaceFormat.Color);
              }

              var normals = new Vector4[heights.Length];

              //for (int y = 0; y < height; y++)
              Parallel.For(0, textureHeight, y =>
              {
            int yMinus1 = (y > 0) ? y - 1 : 0;
            int yPlus1 = (y < h - 2) ? y + 1 : y;
            for (int x = 0; x < w; x++)
            {
              // The normals are computed using the Sobel filter kernel:
              //  -1 0 +1
              //  -2 0 +2
              //  -1 0 +1
              float deltaX = 0;
              float deltaZ = 0;
              int xMinus1 = (x > 0) ? x - 1 : x;
              int xPlus1 = (x < w - 2) ? x + 1 : x;
              deltaX += 1 * (heights[yMinus1 * w + xMinus1] - heights[yMinus1 * w + xPlus1]);
              deltaX += 2 * (heights[y * w + xMinus1] - heights[y * w + xPlus1]);
              deltaX += 1 * (heights[yPlus1 * w + xMinus1] - heights[yPlus1 * w + xPlus1]);

              deltaZ += 1 * (heights[yMinus1 * w + xMinus1] - heights[yPlus1 * w + xMinus1]);
              deltaZ += 2 * (heights[yMinus1 * w + x] - heights[yPlus1 * w + x]);
              deltaZ += 1 * (heights[yMinus1 * w + xPlus1] - heights[yPlus1 * w + xPlus1]);

              deltaX /= 4.0f;
              deltaZ /= 4.0f;

              // Instead of the cross product we can compute the normal directly.
              // See derivation in "Fast Heightfield Normal Calculation", Game Programming Gems 3, pp. 344.
              Vector3F normal = new Vector3F(deltaX, 2 * cellSize, deltaZ);

              normal.TryNormalize();

              // Change order of z and y to standard normal map order.
              // Convert [-1, 1] range to [0, 1].
              normals[y * w + x] = new Vector4(
            normal.X / 2.0f + 0.5f,
            -normal.Z / 2.0f + 0.5f,    // Invert to create standard "green-up" normal maps.
            normal.Y / 2.0f + 0.5f,
            1);
            }
              });

              SetTextureLevel(normalTexture, 0, normals);
              CreateTerrainGeometryMipLevels(normalTexture, normals, useNearestNeighborFilter);
        }

        public void TryNormalize()
        {
            Vector3F v = Vector3F.Zero;
              bool normalized = v.TryNormalize();
              Assert.IsFalse(normalized);

              v = new Vector3F(1, 2, 3);
              normalized = v.TryNormalize();
              Assert.IsTrue(normalized);
              Assert.AreEqual(new Vector3F(1, 2, 3).Normalized, v);

              v = new Vector3F(0, -1, 0);
              normalized = v.TryNormalize();
              Assert.IsTrue(normalized);
              Assert.AreEqual(new Vector3F(0, -1, 0).Normalized, v);
        }

        //--------------------------------------------------------------
        /// <inheritdoc/>
        protected override void OnSetup()
        {
            // Get anchor orientations in world space.
              Matrix33F anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal;
              Matrix33F anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal;

              // Get the quaternion that rotates something from anchor orientation A to
              // anchor orientation B:
              //   QB = QTotal * QA
              //   => QTotal = QB * QA.Inverse
              QuaternionF total = QuaternionF.CreateRotation(anchorOrientationB * anchorOrientationA.Transposed);

              // Compute swing axis and angle.
              Vector3F xAxisA = anchorOrientationA.GetColumn(0);
              Vector3F yAxisA = anchorOrientationA.GetColumn(1);
              Vector3F xAxisB = anchorOrientationB.GetColumn(0);
              QuaternionF swing = QuaternionF.CreateRotation(xAxisA, xAxisB);

              Vector3F swingAxis = new Vector3F(swing.X, swing.Y, swing.Z);
              if (!swingAxis.TryNormalize())
            swingAxis = yAxisA;

              float swingAngle = swing.Angle;

              Debug.Assert(
            0 <= swingAngle && swingAngle <= ConstantsF.Pi,
            "QuaternionF.CreateRotation(Vector3F, Vector3F) should only create rotations along the \"short arc\".");

              // The swing limits create a deformed cone. If we look onto the x-axis of A:
              // y-axis goes to the right. z-axis goes up.
              Vector3F xAxisBInAnchorA = Matrix33F.MultiplyTransposed(anchorOrientationA, xAxisB);
              float directionY = xAxisBInAnchorA.Y;
              float directionZ = xAxisBInAnchorA.Z;

              // In this plane, we have an ellipse with the formula:
              //   y²/a² + z²/b² = 1, where a and b are the ellipse radii.
              // We don't know the exact radii. We can compute them from the swing min/max angles.
              // To make it simpler, we do not use a flat ellipse. We use the swing z limit for a.
              // And the swing y limit for b.
              // We have a different ellipse for each quarter.
              float ellipseA = (directionY > 0) ? Maximum.Z : -Minimum.Z;
              float ellipseB = (directionZ > 0) ? -Minimum.Y : Maximum.Y;

              // The angles are in radians: angle = bow/radius. So our a and b are on the unit sphere.
              // This creates an elliptic thing on the unit sphere - not in a plane. We don't care because
              // we only need a smooth interpolation between the swing y and z limits.
              // No we look for the swing angle in the direction of xAxisB.
              // The next step can derived from following formulas:
              //     y²/a² + z²/b² = 1                   The ellipse formula.
              //     slope = directionZ / directionY     The direction in which we need the limit.
              //     slope = z/y                         The (y,z) is the point on the ellipse in the given direction.
              //     swingLimit = sqrt(y² + z²)          This is the distance of (y,z) from the center.
              // Since our ellipse is on a sphere, swingLimit is an angle (= bow / radius).

              float swingLimit = ellipseB;
              if (!Numeric.IsZero(directionY))
              {
            float slope = directionZ / directionY;
            float slopeSquared = slope * slope;
            float ellipseASquared = ellipseA * ellipseA;
            float ellipseBSquared = ellipseB * ellipseB;
            swingLimit = (float)Math.Sqrt((1 + slopeSquared) / (1 / ellipseASquared + slopeSquared / ellipseBSquared));

            // The ellipse normal would give us a better swing axis. But our computed swingAngle
            // is not correct for this axis...
            // Create a swing axis from the ellipse normal.
            //float k = ellipseASquared / ellipseBSquared * directionZ / directionY;
            //var normal = anchorOrientationA * new Vector3F(0, -k, 1).Normalized;
            //if (Vector3F.Dot(normal, swingAxis) < 0)
            //  swingAxis = -normal;
            //else
            //  swingAxis = normal;
              }

            #if DEBUG
              //Debug.Assert(QuaternionF.Dot(swing, total) >= 0);
              var swingAxisALocal = Matrix33F.MultiplyTransposed(anchorOrientationA, swingAxis);
              Debug.Assert(Numeric.IsZero(swingAxisALocal.X));
            #endif

              // We define our rotations like this:
              // First we twist around the x-axis of A. Then we swing.
              //   QTotal = QSwing * QTwist
              //   => QSwing.Inverse * QTotal = QTwist
              QuaternionF twist = swing.Conjugated * total;
              twist.Normalize();

              // The quaternion returns an angle in the range [0, 2π].
              float twistAngle = twist.Angle;

              // The minimum and maximum twist limits are in the range [-π, π].
              if (twistAngle > ConstantsF.Pi)
              {
            // Convert the twistAngle to the range used by the twist limits.
            twistAngle = -(ConstantsF.TwoPi - twistAngle);
            Debug.Assert(-ConstantsF.TwoPi < twistAngle && twistAngle <= ConstantsF.TwoPi);
              }

//.........这里部分代码省略.........

        private void Initialize(RigidBody bodyA, RigidBody bodyB, Contact contact)
        {
            Debug.Assert(bodyA != null && bodyB != null && contact != null);
              _bodyA = bodyA;
              _bodyB = bodyB;
              Contact = contact;

              _simulation = _bodyA.Simulation ?? _bodyB.Simulation;
              Debug.Assert(_simulation != null);

              ErrorReduction = _simulation.Settings.Constraints.ContactErrorReduction;
              _minConstraintImpulse = _simulation.Settings.Constraints.MinConstraintImpulse;

              Vector3F n = Contact.Normal;
              _rA = Contact.PositionAWorld - BodyA.PoseCenterOfMass.Position;
              _rB = Contact.PositionBWorld - BodyB.PoseCenterOfMass.Position;
              _rALengthSquared = -1;
              _rBLengthSquared = -1;

              Vector3F vRel = RelativeVelocity;
              float vRelN = Vector3F.Dot(vRel, n);

              // Get material-dependent values.
              var materialA = BodyA.Material.GetProperties(BodyA, Contact.PositionALocal, Contact.FeatureA);
              var materialB = BodyB.Material.GetProperties(BodyB, Contact.PositionBLocal, Contact.FeatureB);
              var materialCombiner = _simulation.Settings.MaterialPropertyCombiner;

              // Restitution is clamped to 0 if bodies are slow or if restitution is very low.
              if (Math.Abs(vRelN) > _simulation.Settings.Constraints.RestingVelocityLimit)
              {
            _restitution = materialCombiner.CombineRestitution(materialA.Restitution, materialB.Restitution);
            if (_restitution < _simulation.Settings.Constraints.RestitutionThreshold)
              _restitution = 0;
              }
              else
              {
            // The contacts must be created BEFORE forces are applied! If the bodies are moving less
            // than the RestingVelocityLimit, we set the Restitution to zero.
            // Bodies that do not move relative to each other must have a restitution of 0 to get
            // stable stacks!
            // RestingVelocityLimit can even be zero!
            _restitution = 0;
              }

              _staticFriction = materialCombiner.CombineFriction(materialA.StaticFriction, materialB.StaticFriction);
              _dynamicFriction = materialCombiner.CombineFriction(materialA.DynamicFriction, materialB.DynamicFriction);
              _surfaceMotionAEnabled = materialA.SupportsSurfaceMotion;
              _surfaceMotionBEnabled = materialB.SupportsSurfaceMotion;

              // Get friction directions. The first direction is in direction of the greatest tangent
              // velocity.
              _t0 = vRel - vRelN * n;
              if (!_t0.TryNormalize())
            _t0 = n.Orthonormal1;

              _t1 = Vector3F.Cross(_t0, n);

              // Reset cached impulses.
              _penetrationConstraint.ConstraintImpulse = 0;
              _frictionConstraint0.ConstraintImpulse = 0;
              _frictionConstraint1.ConstraintImpulse = 0;
        }

    /// <summary>
    /// Gets a support point for a given normalized direction vector.
    /// </summary>
    /// <param name="directionNormalized">
    /// The normalized direction vector for which to get the support point.
    /// </param>
    /// <returns>A support point regarding the given direction.</returns>
    /// <remarks>
    /// A support point regarding a direction is an extreme point of the shape that is furthest away
    /// from the center regarding the given direction. This point is not necessarily unique.
    /// </remarks>
    public override Vector3F GetSupportPointNormalized(Vector3F directionNormalized)
    {
      // The general formula for cylinders with arbitrary orientation is in 
      // Bergen: "Collision Detection in Interactive 3D Environments", pp. 136.
      // The formula for cylinders with up-axis = y-axis is simpler:

      Vector3F directionInXYPlane = new Vector3F(directionNormalized.X, 0, directionNormalized.Z);
      if (directionInXYPlane.TryNormalize())
      {
        // The general case.
        if (directionNormalized.Y >= 0)
          return Vector3F.UnitY * _height / 2 + _radius * directionInXYPlane;
        else
          return -Vector3F.UnitY * _height / 2 + _radius * directionInXYPlane;
      }
      else
      {
        // localDirection == +/-(0, 1, 0)
        Debug.Assert(Numeric.IsZero(directionNormalized.X) && Numeric.IsZero(directionNormalized.Z), "X and Y of direction are expected to be (near) zero.");

        if (directionNormalized.Y >= 0)
          return new Vector3F(_radius, _height / 2, 0);
        else
          return new Vector3F(_radius, -_height / 2, 0);
      }
    }

//.........这里部分代码省略.........
              bool isClosed = Vector3F.AreNumericallyEqual(flattenedPoints[0], flattenedPoints[flattenedPoints.Count - 1]);

              {
            // Get the line segments which of the road border.
            List<Vector4F> segments = new List<Vector4F>();  // 2 points per segment.
            Vector3F lastOrthonormal = Vector3F.Right;
            Vector4F previousV1 = Vector4F.Zero;
            Vector4F previousV2 = Vector4F.Zero;
            for (int i = 0; i < flattenedPoints.Count; i++)
            {
              Vector3F start = flattenedPoints[i];

              Vector3F previous;
              bool isFirstPoint = (i == 0);
              if (!isFirstPoint)
            previous = flattenedPoints[i - 1];
              else if (isClosed && road.SmoothEnds)
            previous = flattenedPoints[flattenedPoints.Count - 2];
              else
            previous = start;

              Vector3F next;
              bool isLastPoint = (i + 1 == flattenedPoints.Count);
              if (!isLastPoint)
            next = flattenedPoints[i + 1];
              else if (isClosed && road.SmoothEnds)
            next = flattenedPoints[1];
              else
            next = start;

              Vector3F tangent = next - previous;

              Vector3F orthonormal = new Vector3F(tangent.Z, 0, -tangent.X);
              if (!orthonormal.TryNormalize())
            orthonormal = lastOrthonormal;

              // Add 2 vertices two segments for the road side border.
              //
              //  pV1        pV2 (previous vertices)
              //  x           x
              //  |           |
              //  x           x
              //  v1          v2 (current vertices)
              //
              // We store the side falloff with the vertex:
              // Vectors are 4D. Height is y. Side falloff is w.
              Vector4F v1 = new Vector4F(start - orthonormal * (halfWidths[i] + 0), sideFalloffs[i]);
              Vector4F v2 = new Vector4F(start + orthonormal * (halfWidths[i] + 0), sideFalloffs[i]);

              if (i > 0)
              {
            segments.Add(previousV1);
            segments.Add(v1);
            segments.Add(previousV2);
            segments.Add(v2);

            if (isLastPoint && !isClosed)
            {
              // A segment for the end of the road.
              segments.Add(v1);
              segments.Add(v2);
            }
              }
              else
              {
            if (!isClosed)

    /// <summary>
    /// Gets a support point for a given direction.
    /// </summary>
    /// <param name="direction">
    /// The direction for which to get the support point. The vector does not need to be normalized.
    /// The result is undefined if the vector is a zero vector.
    /// </param>
    /// <returns>A support point regarding the given direction.</returns>
    /// <remarks>
    /// <para>
    /// A support point regarding a direction is an extreme point of the shape that is furthest away
    /// from the center regarding the given direction. This point is not necessarily unique.
    /// </para>
    /// </remarks>
    public override Vector3F GetSupportPoint(Vector3F direction)
    {
      direction.Z = 0;

      if (!direction.TryNormalize())
        direction = Vector3F.UnitX;

      return direction * _radius;
    }

        /// <summary>
        /// Prepares a normal map for compression using DXT5 (a.k.a. DXT5nm).
        /// </summary>
        /// <param name="image">The image (<see cref="DataFormat.R32G32B32A32_FLOAT"/>).</param>
        /// <param name="invertY"><see langword="true"/> to invert the y component.</param>
        /// <exception cref="ArgumentNullException">
        /// <paramref name="image"/> is <see langword="null"/>.
        /// </exception>
        public static void ProcessNormals(Image image, bool invertY)
        {
            if (image == null)
            throw new ArgumentNullException("image");

              float sign = invertY ? -1 : 1;

              // ReSharper disable AccessToDisposedClosure
              using (var image4F = new ImageAccessor(image))
              {
            #if SINGLE_THREADED
            for (int y = 0; y < image4F.Height; y++)
            #else
            Parallel.For(0, image4F.Height, y =>
            #endif
            {
              for (int x = 0; x < image4F.Width; x++)
              {
            Vector4F v = image4F.GetPixel(x, y);
            Vector3F normal = new Vector3F(v.X, v.Y, v.Z);

            // Renormalize normals. (Important for higher mipmap levels.)
            if (!normal.TryNormalize())
              normal = new Vector3F(0, 0, 1);

            // Convert to DXT5nm (xGxR).
            v.X = 0.0f;
            v.Y = sign * normal.Y * 0.5f + 0.5f;
            v.Z = 0.0f;
            v.W = normal.X * 0.5f + 0.5f;

            image4F.SetPixel(x, y, v);
              }
            }
            #if !SINGLE_THREADED
            );
            #endif
              }
              // ReSharper restore AccessToDisposedClosure
        }

        //--------------------------------------------------------------
        private void InitializeBody(Vector3F upVector)
        {
            if (!upVector.TryNormalize())
            throw new ArgumentException("The up vector must not be a zero vector.");

              UpVector = upVector;

              CapsuleShape shape = new CapsuleShape(0.4f, 1.8f);
              MassFrame mass = new MassFrame { Mass = 100 };

              UniformMaterial material = new UniformMaterial
              {
            // The body should be frictionless, so that it can be easily pushed by the simulation to
            // valid positions.
            StaticFriction = 0.0f,
            DynamicFriction = 0.0f,

            // The body should not bounce when being hit or pushed.
            Restitution = 0
              };

              Body = new RigidBody(shape, mass, material)
              {
            // We set the mass explicitly and it should not automatically change when the
            // shape is changed; e.g. a ducked character has a smaller shape, but still the same mass.
            AutoUpdateMass = false,

            // This body is under our control and should never be deactivated by the simulation.
            CanSleep = false,
            CcdEnabled = true,

            // The capsule does not rotate in any direction.
            LockRotationX = true,
            LockRotationY = true,
            LockRotationZ = true,

            Name = "CharacterController",

            Pose = new Pose(shape.Height / 2 * upVector,
                        QuaternionF.CreateRotation(Vector3F.UnitY, upVector)),
              };

              // When the user changes the shape, we must re-compute all contacts.
              Body.ShapeChanged += (s, e) => UpdateContacts();
        }

    /// <overloads>
    /// <summary>
    /// Gets a support point for a given direction.
    /// </summary>
    /// </overloads>
    /// 
    /// <summary>
    /// Gets a support point for a given direction.
    /// </summary>
    /// <param name="direction">
    /// The direction for which to get the support point. The vector does not need to be normalized.
    /// The result is undefined if the vector is a zero vector.
    /// </param>
    /// <returns>A support point regarding the given direction.</returns>
    /// <remarks>
    /// <para>
    /// A support point regarding a direction is an extreme point of the shape that is furthest away 
    /// from the center regarding the given direction. This point is not necessarily unique.
    /// </para>
    /// <para>
    /// <strong>Notes to Inheritors:</strong>
    /// The default implementation of this method normalizes the direction and calls
    /// <see cref="GetSupportPointNormalized"/>. If this method is overridden,
    /// don't forget to check whether direction is a zero vector and to normalize 
    /// <paramref name="direction"/> if required.
    /// </para>
    /// </remarks>
    public virtual Vector3F GetSupportPoint(Vector3F direction)
    {
      if (!direction.TryNormalize())
        direction = Vector3F.UnitX;

      return GetSupportPointNormalized(direction);
    }

        //--------------------------------------------------------------
        /// <inheritdoc/>
        protected override void OnSetup()
        {
            var deltaTime = Simulation.Settings.Timing.FixedTimeStep;
              float errorReduction = ConstraintHelper.ComputeErrorReduction(deltaTime, SpringConstant, DampingConstant);
              float softness = ConstraintHelper.ComputeSoftness(deltaTime, SpringConstant, DampingConstant);

              // Get anchor orientations in world space.
              Matrix33F anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal;
              Matrix33F anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal;

              Matrix33F relativeOrientationMatrix = anchorOrientationA.Transposed * anchorOrientationB;
              QuaternionF relativeOrientation = QuaternionF.CreateRotation(relativeOrientationMatrix);
              QuaternionF deltaRotation = TargetOrientation * relativeOrientation.Conjugated;

              float angle = deltaRotation.Angle;
              if (angle > ConstantsF.Pi)
              {
            // Quaternion should be the shortest arc quaternion (angle < 180°).
            deltaRotation = -deltaRotation;
            angle = ConstantsF.TwoPi - angle;
            Debug.Assert(Numeric.AreEqual(angle, deltaRotation.Angle));
              }

              Vector3F axis = new Vector3F(deltaRotation.X, deltaRotation.Y, deltaRotation.Z);
              if (!axis.TryNormalize())
              {
            // Do nothing.
            _minImpulseLimits[0] = 0;
            _minImpulseLimits[1] = 0;
            _minImpulseLimits[2] = 0;
              }
              else
              {
            // Axis is in local space of anchor A.
            // Convert axis to world space.
            axis = anchorOrientationA * axis;

            // Main axis is the quaternion axis.
            bool isActive = !Numeric.IsZero(angle);
            SetupConstraint(0, -angle, 0, axis, deltaTime, errorReduction, softness, isActive);
            if (!UseSingleAxisMode)
            {
              // In multi-axes-mode mode: constrain rotation on 2 orthogonal axes.
              SetupConstraint(1, 0, 0, axis.Orthonormal1, deltaTime, errorReduction, softness, isActive);
              SetupConstraint(2, 0, 0, axis.Orthonormal2, deltaTime, errorReduction, softness, isActive);
            }
              }

              // No warmstarting.
              _constraints[0].ConstraintImpulse = 0;
              _constraints[1].ConstraintImpulse = 0;
              _constraints[2].ConstraintImpulse = 0;
        }

        //--------------------------------------------------------------
        /// <inheritdoc/>
        protected override void OnSetup()
        {
            _deltaTime = Simulation.Settings.Timing.FixedTimeStep;

              // Anchors in world space.
              var anchorA = BodyA.Pose.ToWorldPosition(AnchorPositionALocal);
              var anchorB = BodyB.Pose.ToWorldPosition(AnchorPositionBLocal);

              // The constraint acts on the axis between the anchors.
              _axis = anchorB - anchorA;

              float distance = _axis.Length;

              // Check if limits are active.
              _minLimitIsActive = distance < MinDistance && !Numeric.IsZero(distance);
              _maxLimitIsActive = distance > MaxDistance;

              // Abort if no limit is active.
              if (!_minLimitIsActive && !_maxLimitIsActive)
              {
            _constraint.ConstraintImpulse = 0;
            return;
              }

              _axis.TryNormalize();

              _ra = anchorA - BodyA.PoseCenterOfMass.Position;
              _rb = anchorB - BodyB.PoseCenterOfMass.Position;

              // Too close together = positive deviation and positive target velocity.
              // Too far apart = negative deviation and negative target velocity.
              var deviation = (_minLimitIsActive) ? MinDistance - distance : MaxDistance - distance;
              _constraint.TargetRelativeVelocity = deviation * ErrorReduction / _deltaTime;

              float maxErrorCorrectionVelocity = Simulation.Settings.Constraints.MaxErrorCorrectionVelocity;
              _constraint.TargetRelativeVelocity = MathHelper.Clamp(_constraint.TargetRelativeVelocity, -maxErrorCorrectionVelocity, maxErrorCorrectionVelocity);

              _constraint.Softness = Softness / _deltaTime;
              _constraint.Prepare(BodyA, BodyB, -_axis, -Vector3F.Cross(_ra, _axis), _axis, Vector3F.Cross(_rb, _axis));

              // To keep it simple we do not warmstart. Warmstarting can only be done if the same limit
              // was active the last time.
              _constraint.ConstraintImpulse = 0;
        }

    /// <summary>
    /// Initializes a new instance of the <see cref="PlaneShape"/> class from three points.
    /// </summary>
    /// <param name="point0">A point on the plane.</param>
    /// <param name="point1">A point on the plane.</param>
    /// <param name="point2">A point on the plane.</param>
    /// <remarks>
    /// This constructor creates a <see cref="PlaneShape"/> from three points in the plane. The
    /// points must be ordered counter-clockwise. The front-face (which points into the empty
    /// half-space) is defined through the counter-clockwise order of the points.
    /// </remarks>
    /// <exception cref="ArgumentException">
    /// <paramref name="point0"/>, <paramref name="point1"/>, and <paramref name="point2"/> do not 
    /// form a valid triangle.
    /// </exception>
    public PlaneShape(Vector3F point0, Vector3F point1, Vector3F point2)
    {
      if (Vector3F.AreNumericallyEqual(point0, point1)
          || Vector3F.AreNumericallyEqual(point0, point2)
          || Vector3F.AreNumericallyEqual(point1, point2))
        throw new ArgumentException("The points do not form a valid triangle.");

      // Compute normal vector.
      _normal = Vector3F.Cross(point1 - point0, point2 - point0);

      if (!_normal.TryNormalize())
        throw new ArgumentException("The points do not form a valid triangle.");

      // Compute the distance from the origin.
      _distanceFromOrigin = Vector3F.Dot(point0, _normal);
    }

    /// <summary>
    /// Gets a support point for a given normalized direction vector.
    /// </summary>
    /// <param name="directionNormalized">
    /// The normalized direction vector for which to get the support point.
    /// </param>
    /// <returns>A support point regarding the given direction.</returns>
    /// <remarks>
    /// A support point regarding a direction is an extreme point of the shape that is furthest away
    /// from the center regarding the given direction. This point is not necessarily unique.
    /// </remarks>
    public override Vector3F GetSupportPointNormalized(Vector3F directionNormalized)
    {
      directionNormalized.Z = 0;

      if (!directionNormalized.TryNormalize())
        directionNormalized = Vector3F.UnitX;

      return directionNormalized * _radius;
    }