/// <summary>
        /// Constructs a new character controller with the most common configuration options.
        /// </summary>
        /// <param name="position">Initial position of the character.</param>
        /// <param name="height">Height of the character body while standing.</param>
        /// <param name="crouchingHeight">Height of the character body while crouching.</param>
        /// <param name="radius">Radius of the character body.</param>
        /// <param name="mass">Mass of the character body.</param>
        public CharacterController(Vector3 position, float height, float crouchingHeight, float radius, float mass)
        {
            Body = new Cylinder(position, height, radius, mass);
            Body.IgnoreShapeChanges = true; //Wouldn't want inertia tensor recomputations to occur when crouching and such.
            Body.CollisionInformation.Shape.CollisionMargin = .1f;
            //Making the character a continuous object prevents it from flying through walls which would be pretty jarring from a player's perspective.
            Body.PositionUpdateMode        = PositionUpdateMode.Continuous;
            Body.LocalInertiaTensorInverse = new Matrix3x3();
            //TODO: In v0.16.2, compound bodies would override the material properties that get set in the CreatingPair event handler.
            //In a future version where this is changed, change this to conceptually minimally required CreatingPair.
            Body.CollisionInformation.Events.DetectingInitialCollision += RemoveFriction;
            Body.LinearDamping         = 0;
            SupportFinder              = new SupportFinder(this);
            HorizontalMotionConstraint = new HorizontalMotionConstraint(this);
            VerticalMotionConstraint   = new VerticalMotionConstraint(this);
            StepManager   = new StepManager(this);
            StanceManager = new StanceManager(this, crouchingHeight);
            QueryManager  = new QueryManager(this);

            //Enable multithreading for the characters.
            IsUpdatedSequentially = false;
            //Link the character body to the character controller so that it can be identified by the locker.
            //Any object which replaces this must implement the ICharacterTag for locking to work properly.
            Body.CollisionInformation.Tag = new CharacterSynchronizer(Body);
        }
 public override void OnRemovalFromSpace(ISpace oldSpace)
 {
     //Remove any supplements from the space too.
     oldSpace.Remove(Body);
     oldSpace.Remove(HorizontalMotionConstraint);
     oldSpace.Remove(VerticalMotionConstraint);
     //This character controller requires the standard implementation of Space.
     ((Space)oldSpace).BoundingBoxUpdater.Finishing -= ExpandBoundingBox;
     SupportFinder.ClearSupportData();
     Body.AngularVelocity = new Vector3();
     Body.LinearVelocity  = new Vector3();
 }
        void IBeforeSolverUpdateable.Update(float dt)
        {
            //Someone may want to use the Body.CollisionInformation.Tag for their own purposes.
            //That could screw up the locking mechanism above and would be tricky to track down.
            //Consider using the making the custom tag implement ICharacterTag, modifying LockCharacterPairs to analyze the different Tag type, or using the Entity.Tag for the custom data instead.
            Debug.Assert(Body.CollisionInformation.Tag is ICharacterTag, "The character.Body.CollisionInformation.Tag must implement ICharacterTag to link the CharacterController and its body together for character-related locking to work in multithreaded simulations.");

            //We can't let multiple characters manage the same pairs simultaneously.  Lock it up!
            LockCharacterPairs();
            try
            {
                CorrectContacts();

                bool hadSupport = SupportFinder.HasSupport;

                CollectSupportData();


                //Compute the initial velocities relative to the support.
                Vector3 relativeVelocity;
                ComputeRelativeVelocity(ref supportData, out relativeVelocity);
                float verticalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);


                //Don't attempt to use an object as support if we are flying away from it (and we were never standing on it to begin with).
                if (SupportFinder.HasSupport && !hadSupport && verticalVelocity < 0)
                {
                    SupportFinder.ClearSupportData();
                    supportData = new SupportData();
                }


                //Attempt to jump.
                if (tryToJump && StanceManager.CurrentStance != Stance.Crouching) //Jumping while crouching would be a bit silly.
                {
                    //In the following, note that the jumping velocity changes are computed such that the separating velocity is specifically achieved,
                    //rather than just adding some speed along an arbitrary direction.  This avoids some cases where the character could otherwise increase
                    //the jump speed, which may not be desired.
                    if (SupportFinder.HasTraction)
                    {
                        //The character has traction, so jump straight up.
                        float currentUpVelocity = Vector3.Dot(Body.OrientationMatrix.Up, relativeVelocity);
                        //Target velocity is JumpSpeed.
                        float velocityChange = Math.Max(jumpSpeed - currentUpVelocity, 0);
                        ApplyJumpVelocity(ref supportData, Body.OrientationMatrix.Up * velocityChange, ref relativeVelocity);


                        //Prevent any old contacts from hanging around and coming back with a negative depth.
                        foreach (var pair in Body.CollisionInformation.Pairs)
                        {
                            pair.ClearContacts();
                        }
                        SupportFinder.ClearSupportData();
                        supportData = new SupportData();
                    }
                    else if (SupportFinder.HasSupport)
                    {
                        //The character does not have traction, so jump along the surface normal instead.
                        float currentNormalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
                        //Target velocity is JumpSpeed.
                        float velocityChange = Math.Max(slidingJumpSpeed - currentNormalVelocity, 0);
                        ApplyJumpVelocity(ref supportData, supportData.Normal * -velocityChange, ref relativeVelocity);

                        //Prevent any old contacts from hanging around and coming back with a negative depth.
                        foreach (var pair in Body.CollisionInformation.Pairs)
                        {
                            pair.ClearContacts();
                        }
                        SupportFinder.ClearSupportData();
                        supportData = new SupportData();
                    }
                }
                tryToJump = false;


                //Try to step!
                Vector3 newPosition;
                if (StepManager.TryToStepDown(out newPosition) ||
                    StepManager.TryToStepUp(out newPosition))
                {
                    TeleportToPosition(newPosition, dt);
                }

                if (StanceManager.UpdateStance(out newPosition))
                {
                    TeleportToPosition(newPosition, dt);
                }
            }
            finally
            {
                UnlockCharacterPairs();
            }

            //if (SupportFinder.HasTraction && SupportFinder.Supports.Count == 0)
            //{
            //There's another way to step down that is a lot cheaper, but less robust.
            //This modifies the velocity of the character to make it fall faster.
            //Impacts with the ground will be harder, so it will apply superfluous force to supports.
            //Additionally, it will not be consistent with instant up-stepping.
            //However, because it does not do any expensive queries, it is very fast!

            ////We are being supported by a ray cast, but we're floating.
            ////Let's try to get to the ground faster.
            ////How fast?  Try picking an arbitrary velocity and setting our relative vertical velocity to that value.
            ////Don't go farther than the maximum distance, though.
            //float maxVelocity = (SupportFinder.SupportRayData.Value.HitData.T - SupportFinder.RayLengthToBottom);
            //if (maxVelocity > 0)
            //{
            //    maxVelocity = (maxVelocity + .01f) / dt;

            //    float targetVerticalVelocity = -3;
            //    verticalVelocity = Vector3.Dot(Body.OrientationMatrix.Up, relativeVelocity);
            //    float change = MathHelper.Clamp(targetVerticalVelocity - verticalVelocity, -maxVelocity, 0);
            //    ChangeVelocityUnilaterally(Body.OrientationMatrix.Up * change, ref relativeVelocity);
            //}
            //}



            //Vertical support data is different because it has the capacity to stop the character from moving unless
            //contacts are pruned appropriately.
            SupportData verticalSupportData;
            Vector3     movement3d = new Vector3(HorizontalMotionConstraint.MovementDirection.X, 0, HorizontalMotionConstraint.MovementDirection.Y);

            SupportFinder.GetTractionInDirection(ref movement3d, out verticalSupportData);


            //Warning:
            //Changing a constraint's support data is not thread safe; it modifies simulation islands!
            //If something other than a CharacterController can modify simulation islands is running
            //simultaneously (in the IBeforeSolverUpdateable.Update stage), it will need to be synchronized.

            //We don't need to synchronize this all the time- only when the support object changes.
            bool needToLock = HorizontalMotionConstraint.SupportData.SupportObject != supportData.SupportObject ||
                              VerticalMotionConstraint.SupportData.SupportObject != verticalSupportData.SupportObject;

            if (needToLock)
            {
                CharacterSynchronizer.ConstraintAccessLocker.Enter();
            }

            HorizontalMotionConstraint.SupportData = supportData;
            VerticalMotionConstraint.SupportData   = verticalSupportData;

            if (needToLock)
            {
                CharacterSynchronizer.ConstraintAccessLocker.Exit();
            }
        }