C# Vector3.LengthSquared方法代码示例

		// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp
		//public class Hull m_optionalHull;

		public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
		{
			Vector3 supVec = new Vector3();

			float maxDot = -1e30f;

			float lenSqr = vec.LengthSquared();
			if (lenSqr < 0.0001f)
			{
				vec = new Vector3(1, 0, 0);
			}
			else
			{
				float rlen = 1f / (float)Math.Sqrt(lenSqr);
				vec *= rlen;
			}

			Vector3 vtx;
			float newDot;

			for (int i = 0; i < VertexCount; i++)
			{
				GetVertex(i, out vtx);
				newDot = Vector3.Dot(vec, vtx);
				if (newDot > maxDot)
				{
					maxDot = newDot;
					supVec = vtx;
				}
			}
			return supVec;
		}

 public void Vector3_CalculatesLengthSquaredCorrectly()
 {
     var vectorX = 123.4f;
     var vectorY = 567.8f;
     var vectorZ = 901.2f;
     var vector  = new Vector3(vectorX, vectorY, vectorZ);
     
     TheResultingValue(vector.LengthSquared()).WithinDelta(0.1f)
         .ShouldBe((vectorX * vectorX) + (vectorY * vectorY) + (vectorZ * vectorZ));
 }

        public static bool AddPrefab(string prefabName, long positionEntityId, bool wireframe, ulong messageId = 0)
        {
            if (!MyAPIGateway.Entities.EntityExists(positionEntityId))
                return false;

            var entity = MyAPIGateway.Entities.GetEntityById(positionEntityId);

            var prefab = MyDefinitionManager.Static.GetPrefabDefinition(prefabName);
            if (prefab.CubeGrids == null)
            {
                MyDefinitionManager.Static.ReloadPrefabsFromFile(prefab.PrefabPath);
                prefab = MyDefinitionManager.Static.GetPrefabDefinition(prefab.Id.SubtypeName);
            }

            if (prefab.CubeGrids.Length == 0)
                return false;

            var worldMatrix = entity.WorldMatrix;
            // Use the cubeGrid BoundingBox to determine distance to place.
            Vector3I min = Vector3I.MaxValue;
            Vector3I max = Vector3I.MinValue;
            foreach (var b in prefab.CubeGrids[0].CubeBlocks)
            {
                min = Vector3I.Min(b.Min, min);
                max = Vector3I.Max(b.Min, max);
            }
            var size = new Vector3(max - min);
            var distance = (Math.Sqrt(size.LengthSquared()) * prefab.CubeGrids[0].GridSizeEnum.ToGridLength() / 2) + 2;
            var position = worldMatrix.Translation + worldMatrix.Forward * distance; // offset the position out in front of player by 2m.
            var offset = position - prefab.CubeGrids[0].PositionAndOrientation.Value.Position;
            var tempList = new List<MyObjectBuilder_EntityBase>();

            // We SHOULD NOT make any changes directly to the prefab, we need to make a Value copy using Clone(), and modify that instead.
            foreach (var grid in prefab.CubeGrids)
            {
                var gridBuilder = (MyObjectBuilder_CubeGrid)grid.Clone();
                gridBuilder.PositionAndOrientation = new MyPositionAndOrientation(grid.PositionAndOrientation.Value.Position + offset, grid.PositionAndOrientation.Value.Forward, grid.PositionAndOrientation.Value.Up);

                if (wireframe)
                    foreach (var cube in gridBuilder.CubeBlocks)
                    {
                        cube.IntegrityPercent = 0.01f;
                        cube.BuildPercent = 0.01f;
                    }

                tempList.Add(gridBuilder);
            }

            tempList.CreateAndSyncEntities();
            return true;
        }

        ///<summary>
        /// Computes the expansion of the minkowski sum due to margins in a given direction.
        ///</summary>
        ///<param name="marginA">First margin.</param>
        ///<param name="marginB">Second margin.</param>
        ///<param name="direction">Extreme point direction.</param>
        ///<param name="contribution">Margin contribution to the extreme point.</param>
        public static void ExpandMinkowskiSum(float marginA, float marginB, ref Vector3 direction, out Vector3 contribution)
        {
            float lengthSquared = direction.LengthSquared();
            if (lengthSquared > Toolbox.Epsilon)
            {
                //The contribution to the minkowski sum by the margin is:
                //direction * marginA - (-direction) * marginB.
                Vector3.Multiply(ref direction, (marginA + marginB) / (float)Math.Sqrt(lengthSquared), out contribution);

            }
            else
            {
                contribution = new Vector3();
            }
        }

 public bool AddSupportPoint(ref Vector3 newPoint)
 {
     int index = (BitsToIndices[this.simplexBits ^ 15] & 7) - 1;
     this.y[index] = newPoint;
     this.yLengthSq[index] = newPoint.LengthSquared();
     for (int i = BitsToIndices[this.simplexBits]; i != 0; i = i >> 3)
     {
         int num2 = (i & 7) - 1;
         Vector3 vector = this.y[num2] - newPoint;
         this.edges[num2][index] = vector;
         this.edges[index][num2] = Vector3.Negate(vector);
         this.edgeLengthSq[index][num2] = this.edgeLengthSq[num2][index] = vector.LengthSquared();
     }
     this.UpdateDeterminant(index);
     return this.UpdateSimplex(index);
 }

 public bool AddSupportPoint(ref Vector3 newPoint)
 {
     var index = (BitsToIndices[simplexBits ^ 15] & 7) - 1;
     y[index] = newPoint;
     yLengthSq[index] = newPoint.LengthSquared();
     for (var i = BitsToIndices[simplexBits]; i != 0; i = i >> 3)
     {
         var num2 = (i & 7) - 1;
         var vector = y[num2] - newPoint;
         edges[num2][index] = vector;
         edges[index][num2] = -(vector);
         edgeLengthSq[index][num2] = edgeLengthSq[num2][index] = vector.LengthSquared();
     }
     UpdateDeterminant(index);
     return UpdateSimplex(index);
 }

		public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity)
		{
			linearVelocity = (transformB.Translation - transformA.Translation) / timeStep;
            Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);
			Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat);

			Vector3 axis;
			float angle = 2 * (float)Math.Acos(dorn.W);
			axis = new Vector3(dorn.X, dorn.Y, dorn.Z);
			//axis[3] = 0.f;
			//check for axis length
			float len = axis.LengthSquared();
			if (len < MathHelper.Epsilon * MathHelper.Epsilon)
				axis = new Vector3(1f, 0f, 0f);
			else
				axis /= (float)Math.Sqrt(len);

			angularVelocity = axis * angle / timeStep;
		}

        public void AlignClipboardToGravity(Vector3 gravity)
        {
            if (PreviewGrids.Count > 0 && gravity.LengthSquared() > 0.0001f)
            {
                gravity.Normalize();

                //Vector3 gridLeft = PreviewGrids[0].WorldMatrix.Left;
                Vector3 forward = Vector3D.Reject(m_pasteDirForward, gravity);//Vector3.Cross(gravity, gridLeft);

                m_pasteDirForward = forward;
                m_pasteDirUp = -gravity;
                //m_pasteOrientationAngle = 0f;
            }
        }

        internal void MoveAndRotateInternal(Vector3 moveIndicator, Vector2 rotationIndicator, float roll, Vector3 rotationCenter)
        {
            if (Physics == null)
                return;

            if (DebugMode)
                return;

            //Died character
            if (Physics.CharacterProxy == null)
            {
                moveIndicator = Vector3.Zero;
                rotationIndicator = Vector2.Zero;
                roll = 0;
            }

            var jetpack = JetpackComp;
            bool sprint = moveIndicator.Z != 0 && WantsSprint;
            bool walk = WantsWalk;
            bool jump = WantsJump;
            bool jetpackRunning = jetpack != null && jetpack.Running;
            bool canMove = !jetpackRunning && !((m_currentCharacterState == HkCharacterStateType.HK_CHARACTER_IN_AIR || (int)m_currentCharacterState == MyCharacter.HK_CHARACTER_FLYING) && (m_currentJumpTime <= 0)) && (m_currentMovementState != MyCharacterMovementEnum.Died);
            bool canRotate = (jetpackRunning || !((m_currentCharacterState == HkCharacterStateType.HK_CHARACTER_IN_AIR || (int)m_currentCharacterState == MyCharacter.HK_CHARACTER_FLYING) && (m_currentJumpTime <= 0))) && (m_currentMovementState != MyCharacterMovementEnum.Died);

            float acceleration = 0;
            float lastSpeed = m_currentSpeed;
            if (jetpackRunning)
            {
                jetpack.MoveAndRotate(ref moveIndicator, ref rotationIndicator, roll, canRotate);
            }
            else if (canMove || m_movementsFlagsChanged)
            {
                if (moveIndicator.LengthSquared() > 0)
                    moveIndicator = Vector3.Normalize(moveIndicator);

                MyCharacterMovementEnum newMovementState = GetNewMovementState(ref moveIndicator, ref rotationIndicator, ref acceleration, sprint, walk, canMove, m_movementsFlagsChanged);
                SwitchAnimation(newMovementState);

                m_movementsFlagsChanged = false;

                SetCurrentMovementState(newMovementState);
                if (newMovementState == MyCharacterMovementEnum.Sprinting && StatComp != null)
                {
                    StatComp.ApplyModifier("Sprint");
                }

                if (!IsIdle)
                    m_currentWalkDelay = MathHelper.Clamp(m_currentWalkDelay - VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS, 0, m_currentWalkDelay);

                if (canMove)
                {
                    float relativeSpeed = 1.0f;
                    if (MyFakes.ENABLE_CHARACTER_CONTROL_ON_SERVER)
                    {
                        relativeSpeed = Sync.IsServer ? 1.0f : Sync.RelativeSimulationRatio;
                    }
                    m_currentSpeed = LimitMaxSpeed(m_currentSpeed + (m_currentWalkDelay <= 0 ? acceleration * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS : 0), m_currentMovementState, relativeSpeed);
                }

                if (Physics.CharacterProxy != null)
                {
                    Physics.CharacterProxy.PosX = m_currentMovementState != MyCharacterMovementEnum.Sprinting ? -moveIndicator.X : 0;
                    Physics.CharacterProxy.PosY = moveIndicator.Z;
                    Physics.CharacterProxy.Elevate = 0;
                }

                if (canMove && m_currentMovementState != MyCharacterMovementEnum.Jump)
                {
                    int sign = Math.Sign(m_currentSpeed);
                    m_currentSpeed += -sign * m_currentDecceleration * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;

                    if (Math.Sign(sign) != Math.Sign(m_currentSpeed))
                        m_currentSpeed = 0;
                }

                if (Physics.CharacterProxy != null)
                    Physics.CharacterProxy.Speed = m_currentMovementState != MyCharacterMovementEnum.Died ? m_currentSpeed : 0;

                if ((jump && m_currentMovementState != MyCharacterMovementEnum.Jump))
                {
                    PlayCharacterAnimation("Jump", MyBlendOption.Immediate, MyFrameOption.StayOnLastFrame, 0.0f, 1.3f);

                    if (UseNewAnimationSystem)
                        TriggerCharacterAnimationEvent("jump", true);

                    if (StatComp != null)
                    {
                        StatComp.DoAction("Jump");
                        StatComp.ApplyModifier("Jump");
                    }
                    m_currentJumpTime = JUMP_DURATION;
                    SetCurrentMovementState(MyCharacterMovementEnum.Jump);

                    m_canJump = false;
                    m_frictionBeforeJump = Physics.CharacterProxy.GetHitRigidBody().Friction;

                    if (Physics.CharacterProxy != null)
                    {
                        Physics.CharacterProxy.GetHitRigidBody().ApplyForce(1, WorldMatrix.Up * Definition.JumpForce * MyPerGameSettings.CharacterGravityMultiplier * Physics.Mass);
                        Physics.CharacterProxy.Jump = true;
//.........这里部分代码省略.........

 /// <summary>
 /// Determines the distance between a point and a plane..
 /// </summary>
 /// <param name="point">Point to project onto plane.</param>
 /// <param name="normal">Normal of the plane.</param>
 /// <param name="pointOnPlane">Point located on the plane.</param>
 /// <returns>Distance from the point to the plane.</returns>
 public static float GetDistancePointToPlane(ref Vector3 point, ref Vector3 normal, ref Vector3 pointOnPlane)
 {
     Vector3 offset;
     Vector3.Subtract(ref point, ref pointOnPlane, out offset);
     float dot;
     Vector3.Dot(ref normal, ref offset, out dot);
     return dot / normal.LengthSquared();
 }

        void MoveAngular(float pTimestep, ODEPhysicsScene _pParentScene, ODEPrim parent)
        {
            bool ishovering = false;
            d.Vector3 d_angularVelocity = d.BodyGetAngularVel(Body);
            d.Vector3 d_lin_vel_now = d.BodyGetLinearVel(Body);
            d.Quaternion drotq = d.BodyGetQuaternion(Body);
            Quaternion rotq = new Quaternion(drotq.X, drotq.Y, drotq.Z, drotq.W);
            rotq *= m_referenceFrame; //add reference rotation to rotq
            Quaternion irotq = new Quaternion(-rotq.X, -rotq.Y, -rotq.Z, rotq.W);
            Vector3 angularVelocity = new Vector3(d_angularVelocity.X, d_angularVelocity.Y, d_angularVelocity.Z);
            Vector3 linearVelocity = new Vector3(d_lin_vel_now.X, d_lin_vel_now.Y, d_lin_vel_now.Z);

            Vector3 friction = Vector3.Zero;
            Vector3 vertattr = Vector3.Zero;
            Vector3 deflection = Vector3.Zero;
            Vector3 banking = Vector3.Zero;

            //limit maximum rotation speed
            if(angularVelocity.LengthSquared() > 1e3f) {
                angularVelocity = Vector3.Zero;
                d.BodySetAngularVel(Body, angularVelocity.X, angularVelocity.Y, angularVelocity.Z);
            }

            angularVelocity *= irotq; //world to body orientation

            if(m_VhoverTimescale * pTimestep <= 300.0f && m_VhoverHeight > 0.0f) ishovering = true;

            #region Angular motor
            Vector3 motorDirection = Vector3.Zero;

            if(Type == Vehicle.TYPE_BOAT) {
                //keep z flat for boats, no sidediving lol
                Vector3 tmp = new Vector3(0.0f, 0.0f, m_angularMotorDirection.Z);
                m_angularMotorDirection.Z = 0.0f;
                m_angularMotorDirection += tmp * irotq;
            }

            if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering){
                motorDirection = m_angularMotorDirection * 0.34f; //0.3f;
            }

            m_angularMotorVelocity.X = (motorDirection.X - angularVelocity.X) / m_angularMotorTimescale;
            m_angularMotorVelocity.Y = (motorDirection.Y - angularVelocity.Y) / m_angularMotorTimescale;
            m_angularMotorVelocity.Z = (motorDirection.Z - angularVelocity.Z) / m_angularMotorTimescale;
            m_angularMotorDirection *= (1.0f - 1.0f/m_angularMotorDecayTimescale);

            if(m_angularMotorDirection.LengthSquared() > 0.0f)
            {
                if(angularVelocity.X > m_angularMotorDirection.X && m_angularMotorDirection.X >= 0.0f) m_angularMotorVelocity.X = 0.0f;
                if(angularVelocity.Y > m_angularMotorDirection.Y && m_angularMotorDirection.Y >= 0.0f) m_angularMotorVelocity.Y = 0.0f;
                if(angularVelocity.Z > m_angularMotorDirection.Z && m_angularMotorDirection.Z >= 0.0f) m_angularMotorVelocity.Z = 0.0f;

                if(angularVelocity.X < m_angularMotorDirection.X && m_angularMotorDirection.X <= 0.0f) m_angularMotorVelocity.X = 0.0f;
                if(angularVelocity.Y < m_angularMotorDirection.Y && m_angularMotorDirection.Y <= 0.0f) m_angularMotorVelocity.Y = 0.0f;
                if(angularVelocity.Z < m_angularMotorDirection.Z && m_angularMotorDirection.Z <= 0.0f) m_angularMotorVelocity.Z = 0.0f;
            }

            #endregion

            #region friction

            float initialFriction = 0.0001f;
            if(angularVelocity.X > initialFriction) friction.X += initialFriction;
            if(angularVelocity.Y > initialFriction) friction.Y += initialFriction;
            if(angularVelocity.Z > initialFriction) friction.Z += initialFriction;
            if(angularVelocity.X < -initialFriction) friction.X -= initialFriction;
            if(angularVelocity.Y < -initialFriction) friction.Y -= initialFriction;
            if(angularVelocity.Z < -initialFriction) friction.Z -= initialFriction;

            if(angularVelocity.X > 0.0f)
                friction.X += angularVelocity.X * angularVelocity.X / m_angularFrictionTimescale.X;
            else
                friction.X -= angularVelocity.X * angularVelocity.X / m_angularFrictionTimescale.X;

            if(angularVelocity.Y > 0.0f)
                friction.Y += angularVelocity.Y * angularVelocity.Y / m_angularFrictionTimescale.Y;
            else
                friction.Y -= angularVelocity.Y * angularVelocity.Y / m_angularFrictionTimescale.Y;

            if(angularVelocity.Z > 0.0f)
                friction.Z += angularVelocity.Z * angularVelocity.Z / m_angularFrictionTimescale.Z;
            else
                friction.Z -= angularVelocity.Z * angularVelocity.Z / m_angularFrictionTimescale.Z;

            if(Math.Abs(m_angularMotorDirection.X) > 0.01f) friction.X = 0.0f;
            if(Math.Abs(m_angularMotorDirection.Y) > 0.01f) friction.Y = 0.0f;
            if(Math.Abs(m_angularMotorDirection.Z) > 0.01f) friction.Z = 0.0f;

            #endregion

            #region Vertical attraction

            if(m_verticalAttractionTimescale < 300)
            {
                float VAservo = 38.0f / m_verticalAttractionTimescale;
                if(Type == Vehicle.TYPE_CAR) VAservo = 10.0f / m_verticalAttractionTimescale;

                Vector3 verterr = new Vector3(0.0f, 0.0f, 1.0f);
                verterr *= rotq;
                vertattr.X = verterr.Y;
//.........这里部分代码省略.........

        public Vector3 GetAngularVelocity(Vector3 control)
        {
            /*if (m_grid.GridControllers.IsControlledByLocalPlayer || (!m_grid.GridControllers.IsControlledByAnyPlayer && Sync.IsServer) || (false && Sync.IsServer))
            {*/
            // Not checking whether engines are running, since ControlTorque should be 0 when
            // engines are stopped (set by cockpit).
            if (ResourceSink.SuppliedRatio > 0f && m_grid.Physics != null && m_grid.Physics.Enabled && !m_grid.Physics.RigidBody.IsFixed)
            {
                Matrix invWorldRot = m_grid.PositionComp.WorldMatrixInvScaled.GetOrientation();
                Matrix worldRot = m_grid.WorldMatrix.GetOrientation();
                Vector3 localAngularVelocity = Vector3.Transform(m_grid.Physics.AngularVelocity, ref invWorldRot);

                // CH: CAUTION: Don't try to use InertiaTensor, although it might be more intuitive in some cases.
                // I tried it and it's not an inverse of the InverseInertiaTensor! Only the InverseInertiaTensor seems to be correct!
                var invTensor = m_grid.Physics.RigidBody.InverseInertiaTensor;
                Vector3 invTensorVector = new Vector3(invTensor.M11, invTensor.M22, invTensor.M33);
                var minInvTensor = invTensorVector.Min();

                // Max rotation limiter
                float divider = Math.Max(1, minInvTensor * INV_TENSOR_MAX_LIMIT);

                // Calculate the velocity correction torque
                Vector3 correctionTorque = Vector3.Zero;
                Vector3 desiredAcceleration = desiredAcceleration = (m_overrideTargetVelocity - localAngularVelocity) * MyEngineConstants.UPDATE_STEPS_PER_SECOND;

                // The correction is done by overridden gyros and by the remaining power of the controlled gyros
                // This is not entirely physically correct, but it feels good
                float correctionForce = m_maxOverrideForce + m_maxGyroForce * (1.0f - control.Length());

                // This is to ensure that the correction is done uniformly in all axes
                desiredAcceleration = desiredAcceleration * Vector3.Normalize(invTensorVector);

                Vector3 desiredTorque = desiredAcceleration / invTensorVector;
                float framesToDesiredVelocity = desiredTorque.Length() / correctionForce;

                // If we are very close to the target velocity, just set it without applying the torque
                const float minimalBypassVelocity = 0.005f * 0.005f;
                if (framesToDesiredVelocity < 0.5f && m_overrideTargetVelocity.LengthSquared() < minimalBypassVelocity)
                {
                    return m_overrideTargetVelocity;
                }

                if (!Vector3.IsZero(desiredAcceleration, 0.0001f))
                {
                    // The smoothing coefficient is here to avoid the slowdown stopping the ship abruptly, which doesn't look good
                    float smoothingCoeff = 1.0f - 0.8f / (float)Math.Exp(0.5f * framesToDesiredVelocity);
                    correctionTorque = Vector3.ClampToSphere(desiredTorque, correctionForce) * 0.95f * smoothingCoeff + desiredTorque * 0.05f * (1.0f - smoothingCoeff);

                    // A little black magic to make slowdown on large ships bigger
                    if (m_grid.GridSizeEnum == MyCubeSize.Large)
                        correctionTorque *= 2.0f;
                }

                Torque = (control * m_maxGyroForce + correctionTorque) / divider;

                Torque *= ResourceSink.SuppliedRatio;
                if (Torque.LengthSquared() > 0.0001f)
                {
                    // Manually apply torque and use minimal component of inverted inertia tensor to make rotate same in all axes
                    var delta = Torque * new Vector3(minInvTensor) * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
                    var newAngularVelocity = localAngularVelocity + delta;
                    return Vector3.Transform(newAngularVelocity, ref worldRot);
                }

                const float stoppingVelocitySq = 0.0003f * 0.0003f;
                if (control == Vector3.Zero && m_overrideTargetVelocity == Vector3.Zero && m_grid.Physics.AngularVelocity != Vector3.Zero && m_grid.Physics.AngularVelocity.LengthSquared() < stoppingVelocitySq && m_grid.Physics.RigidBody.IsActive)
                {
                    return Vector3.Zero;
                }
            }
            //}

            if (m_grid.Physics != null)
            {
                return m_grid.Physics.AngularVelocity;
            }
            else
            {
                return Vector3.Zero;
            }
        }

        /// <summary>
        /// Calculates soft coeficient at target point
        /// </summary>
        private static float CalculateSoften(float softAreaPlanar, float softAreaVertical, ref Vector3 normal, Vector3 contactToTarget)
        {
            float planeDist = Math.Abs(Vector3.Dot(normal, contactToTarget));
            float flatDist = (float)Math.Sqrt(Math.Max(0, contactToTarget.LengthSquared() - planeDist * planeDist));

            float vertSoft = Math.Max(0, 1 - planeDist / softAreaVertical);
            float flatSoft = Math.Max(0, 1 - flatDist / softAreaPlanar);

            return vertSoft * flatSoft;
        }

        /// <summary>
        /// Updates the movement basis of the horizontal motion constraint.
        /// Should be updated automatically by the character on each time step; other code should not need to call this.
        /// </summary>
        /// <param name="forward">Forward facing direction of the character.</param>
        public void UpdateMovementBasis(ref Vector3 forward)
        {
            Vector3 down = characterBody.orientationMatrix.Down;
            horizontalForwardDirection = forward - down * Vector3.Dot(down, forward);
            float forwardLengthSquared = horizontalForwardDirection.LengthSquared();

            if (forwardLengthSquared < Toolbox.Epsilon)
            {
                //Use an arbitrary direction to complete the basis.
                horizontalForwardDirection = characterBody.orientationMatrix.Forward;
                strafeDirection = characterBody.orientationMatrix.Right;
            }
            else
            {
                Vector3.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection);
                Vector3.Cross(ref down, ref horizontalForwardDirection, out strafeDirection);
                //Don't need to normalize the strafe direction; it's the cross product of two normalized perpendicular vectors.
            }

            Vector3.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d);
            Vector3 strafeComponent;
            Vector3.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent);
            Vector3.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d);
        }

        ///<summary>
        /// Gets the extreme point of the shape in local space in a given direction with margin expansion.
        ///</summary>
        ///<param name="direction">Direction to find the extreme point in.</param>
        ///<param name="extremePoint">Extreme point on the shape.</param>
        public void GetLocalExtremePoint(Vector3 direction, out Vector3 extremePoint)
        {
            GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);

            float directionLength = direction.LengthSquared();
            if (directionLength > Toolbox.Epsilon)
            {
                Vector3.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction);
                Vector3.Add(ref extremePoint, ref direction, out extremePoint);
            }
        }