using System;
using BEPUphysics.Entities;
using Microsoft.Xna.Framework;
using BEPUphysics;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace BEPUphysicsPhoneDemo
{
    public class CharacterController : Updateable
    {

        /// <summary>
        /// Entity whose shape will be used in a convex cast that finds supporting entities for the character or any entities blocking a step-up.
        /// When the character is supported and looking for something to step on, this shape will be cast downward.
        /// To verify that it is valid, it will then be cast upward from the character's head.
        /// </summary>
        Cylinder castingShape;

        /// <summary>
        /// A box positioned relative to the character's body used to identify collision pairs with nearby objects that could be possibly stood upon.
        /// </summary>
        Box feetCollisionPairCollector;
        /// <summary>
        /// The displacement vector from the center of the character body cylinder to the center of the collision pair collector box entity.
        /// </summary>
        Vector3 feetCollisionPairCollectorPositionOffset;
        /// <summary>
        /// The displacement vector from the center of the character body cylinder to the starting position of the convex cast used to find entity supports below the character.
        /// </summary>
        Vector3 feetSupportFinderOffset;
        /// <summary>
        /// A box positioned relative to the character's body used to identify collision pairs with nearby objects that could hit the character's head if it stepped up on something.
        /// </summary>
        Box headCollisionPairCollector;
        /// <summary>
        /// The displacement vector from the center of the character body cylinder to the center of the collision pair collector box entity.
        /// </summary>
        Vector3 headCollisionPairCollectorPositionOffset;
        /// <summary>
        /// The displacement vector from the center of the character body cylinder to the starting position of the convex cast used to find blockages above the character's head when stepping.
        /// </summary>
        Vector3 headBlockageFinderOffset;
        /// <summary>
        /// The maximum distance up or down which the character is able to move without jumping or falling.
        /// </summary>
        float maximumStepHeight;
        /// <summary>
        /// The vertical distance between the ground and the outer collision margin of the character cylinder to maintain.
        /// Should be some relatively small value; if the character cylinder (and its margin) are allowed to hit the ground before the support finder, 
        /// the character may either lose support or become affected by extra frictional forces.
        /// </summary>
        float supportMargin;


        /// <summary>
        /// The character's physical representation that handles iteractions with the environment.
        /// </summary>
        public Cylinder body;
        /// <summary>
        /// Whether or not the character is currently standing on anything.
        /// </summary>
        public bool isSupported = false;
        /// <summary>
        /// Whether or not the character is currently standing on anything that can be walked upon.
        /// False if there exists no support or the support is too heavily sloped, otherwise true.
        /// </summary>
        public bool hasTraction = false;
        /// <summary>
        /// The maximum slope under which walking forces can be applied.
        /// </summary>
        public float maxSlope = MathHelper.PiOver4;
        /// <summary>
        /// Deceleration applied to oppose uncontrolled horizontal movement when the character has a steady foothold on the ground (hasTraction == true).
        /// </summary>
        public float tractionDeceleration = 10f;
        /// <summary>
        /// Deceleration applied to oppose horizontal movement when the character does not have a steady foothold on the ground (hasTraction == false).
        /// </summary>
        public float slidingDeceleration = .3f;


        /// <summary>
        /// The maximum speed that the character can achieve by itself while airborne.
        /// A character can exceed this by launching off the ground at a higher speed, but cannot force itself to accelerate faster than this using air control while airborne.
        /// </summary>
        public float maximumAirborneSpeed = 2;
        /// <summary>
        /// Change in airborne speed per second.
        /// </summary>
        public float airborneAcceleration = 30;


        /// <summary>
        /// Normalized direction which the character tries to move.
        /// </summary>
        public Vector2 movementDirection = Vector2.Zero;
        /// <summary>
        /// Rate of increase in the character's speed in the movementDirection.
        /// </summary>
        public float acceleration = 50f;
        /// <summary>
        /// Maximum speed in the movementDirection that can be attained.
        /// </summary>
        public float maxSpeed = 30;
        /// <summary>
        /// Initial vertical speed when jumping.
        /// </summary>
        public float jumpSpeed = 6;
        /// <summary>
        /// If objects are moving away from each other vertically above this speed, the character will lose its grip on the ground.
        /// </summary>
        float glueSpeed = 5;




        /// <summary>
        /// Gets and sets the position of the character.
        /// </summary>
        public Vector3 position
        {
            get
            {
                //Since the position of the character is not the position of the character's physical cylinder (due to the support margin), it needs to be offset.
                Vector3 bodyPosition = body.centerPosition;
                return new Vector3(bodyPosition.X, bodyPosition.Y + supportMargin / 2, bodyPosition.Z);
            }
            set
            {
                Vector3 offset = position - value;
                body.teleport(ref offset);
                //While the collision pair collectors will follow the body in the following frame, it might be a bit messy just to leave them hanging around until then.
                feetCollisionPairCollector.teleport(ref offset);
                headCollisionPairCollector.teleport(ref offset);
            }
        }



        /// <summary>
        /// Constructs a simple character controller.
        /// </summary>
        /// <param name="position">Location to initially place the character.</param>
        /// <param name="characterHeight">The height of the character.</param>
        /// <param name="characterWidth">The diameter of the character.</param>
        /// <param name="supportHeight">The distance above the ground that the bottom of the character's body floats.</param>
        /// <param name="mass">Total mass of the character.</param>
        public CharacterController(Vector3 position, float characterHeight, float characterWidth, float mass, float maximumStepHeight, float supportMargin, float collisionMargin)
        {
            //Create the physical body of the character. 
            //The character's cylinder height and radius must be shrunk down marginally
            //to take into account the collision margin and support margin while still fitting in the defined character height/width.
            Vector3 bodyPosition = new Vector3(position.X, position.Y + supportMargin / 2, position.Z);
            body = new Cylinder(bodyPosition,
                                characterHeight - 2 * collisionMargin - supportMargin,
                                characterWidth / 2 - collisionMargin,
                                mass);
            body.collisionMargin = collisionMargin;

            feetCollisionPairCollectorPositionOffset = new Vector3(0, -body.height / 2 - supportMargin - collisionMargin, 0);
            feetCollisionPairCollector = new Box(bodyPosition + feetCollisionPairCollectorPositionOffset, characterWidth, maximumStepHeight * 2, characterWidth, 1);
            feetCollisionPairCollector.collisionRules.personal = CollisionRule.noContacts; //Prevents collision detection/contact generation from being run.
            feetCollisionPairCollector.isAffectedByGravity = false;
            feetCollisionPairCollector.collisionRules.specificEntities.Add(body, CollisionRule.noPair); //Prevents the creation of any collision pairs between the body and the collector.
            feetSupportFinderOffset = new Vector3(0, feetCollisionPairCollectorPositionOffset.Y + maximumStepHeight, 0);

            headCollisionPairCollectorPositionOffset = new Vector3(0, (body.height + maximumStepHeight) / 2 + collisionMargin, 0);
            headCollisionPairCollector = new Box(bodyPosition + headCollisionPairCollectorPositionOffset, characterWidth, maximumStepHeight + collisionMargin, characterWidth, 1);
            headCollisionPairCollector.collisionRules.personal = CollisionRule.noContacts; //Prevents collision detection/contact generation from being run.
            headCollisionPairCollector.isAffectedByGravity = false;
            headCollisionPairCollector.collisionRules.specificEntities.Add(body, CollisionRule.noPair); //Prevents the creation of any collision pairs between the body and the collector.
            headBlockageFinderOffset = new Vector3(0, headCollisionPairCollectorPositionOffset.Y - maximumStepHeight / 2 - collisionMargin, 0);

            castingShape = new Cylinder(Vector3.Zero, 0, body.radius + collisionMargin);
            castingShape.collisionMargin = 0;

            this.maximumStepHeight = maximumStepHeight;
            this.supportMargin = supportMargin;

            body.localInertiaTensorInverse = Toolbox.zeroMatrix;
            feetCollisionPairCollector.localInertiaTensorInverse = Toolbox.zeroMatrix;
            headCollisionPairCollector.localInertiaTensorInverse = Toolbox.zeroMatrix;

        }

        /// <summary>
        /// Handles the updating of the character.  Called by the owning space object when necessary.
        /// </summary>
        /// <param name="dt">Simulation seconds since the last update.</param>
        public override void updateAtEndOfUpdate(float dt)
        {
            Entity supportEntity;
            Vector3 supportLocation, supportNormal, supportLocationVelocity;
            float supportDistance;

            if (findSupport(out supportEntity, out supportLocation, out supportNormal, out supportDistance, out supportLocationVelocity))
            {
                isSupported = true;
                support(supportEntity, supportLocation, supportLocationVelocity, supportNormal, supportDistance);
                hasTraction = isSupportSlopeWalkable(supportNormal);
                handleHorizontalMotion(supportEntity, supportLocation, supportLocationVelocity, supportNormal, supportDistance, dt);

            }
            else
            {

                isSupported = false;
                hasTraction = false;
                handleAirborneMotion(dt);
            }

            feetCollisionPairCollector.internalLinearVelocity = body.internalLinearVelocity;
            feetCollisionPairCollector.teleportTo(body.internalCenterPosition + feetCollisionPairCollectorPositionOffset);
            headCollisionPairCollector.internalLinearVelocity = body.internalLinearVelocity;
            headCollisionPairCollector.teleportTo(body.internalCenterPosition + headCollisionPairCollectorPositionOffset);
        }

        /// <summary>
        /// Locates the closest support entity by performing a convex cast at collected candidates.
        /// </summary>
        /// <param name="supportEntity">The closest supporting entity.</param>
        /// <param name="supportLocation">The support location which the character is standing on.</param>
        /// <param name="supportNormal">The normal at the surface where the character is standing..</param>
        /// <param name="supportDistance">Distance from the maximum step height on the character down to where the latest support location was found.</param>
        /// <param name="supportLocationVelocity">Velocity of the support location on the support entity.</param>
        /// <returns>Whether or not a support was located.</returns>
        bool findSupport(out Entity supportEntity, out Vector3 supportLocation, out Vector3 supportNormal, out float supportDistance, out Vector3 supportLocationVelocity)
        {

            //If there's no traction, it shouldn't try to 'step down' anything so there's no need to extend the cast.
            float maximumDistance;
            if (hasTraction)
                maximumDistance = maximumStepHeight * 2;
            else
                maximumDistance = maximumStepHeight;
            //If the object is flying around in the air, it looks a bit weird for it to lock onto a surface when it gets close.  Instead only glue speed while grounded.
            float glueSpeedToUse;
            if (isSupported)
                glueSpeedToUse = glueSpeed;
            else
                glueSpeedToUse = .01f;
            Vector3 sweep = new Vector3(0, -maximumDistance, 0);
            Vector3 startingLocation = body.internalCenterPosition + feetSupportFinderOffset;

            //There are two 'minimums' to keep track of.  
            //  The first is out of all collisions.
            //  This first collision is used to determine what to 'step' to, which means it requires a significant up or down jump to reach.
            //Since the first collision can be invalidated by various factors,
            //  there is also a defined range of support heights right around the character's feet where no validation is necessary.
            //  This allows the character keep standing on whatever it was that it was standing on before the "step" failed.
            Vector3 hitLocation = new Vector3(), stepHitLocation = new Vector3();
            Vector3 hitNormal = new Vector3(), stepHitNormal = new Vector3();
            Entity hitEntity = null, stepHitEntity = null;
            float distance = float.MaxValue, stepDistance = float.MaxValue;


            foreach (CollisionPair pair in feetCollisionPairCollector.collisionPairs)
            {
                Entity candidate;
                //Determine which member of the collision pair is the possible support.
                //The comparisons are all kept on a "parent" as opposed to "collider" level so that interaction with compound shapes is simpler.
                if (pair.colliderA == feetCollisionPairCollector)
                    candidate = pair.colliderB;
                else
                    candidate = pair.colliderA;
                //Ensure that the candidate is a valid supporting entity.
                if (candidate.collisionRules.personal > CollisionRule.normal)
                    continue; //It is invalid!

                float tempToi;
                Vector3 tempHitLocation, tempHitNormal;

                //Fire a convex cast at the candidate and determine some details! 
                Vector3 centerPosition = candidate.internalCenterPosition;
                Quaternion orientation = candidate.internalOrientationQuaternion;
                if (Toolbox.areSweptObjectsColliding(castingShape, candidate,
                                                     ref startingLocation, ref centerPosition, ref Toolbox.identityOrientation, ref orientation,
                                                     0, candidate.collisionMargin, ref sweep, ref Toolbox.zeroVector, //Could use frame velocity as a candidate sweep, but not really important.
                                                     out tempHitLocation, out tempHitNormal, out tempToi))
                {
                    //tempHitNormal *= -1;
                    tempToi *= maximumDistance;
                    if (tempToi < stepDistance)
                    {
                        stepDistance = tempToi;
                        stepHitLocation = tempHitLocation;
                        stepHitNormal = tempHitNormal;
                        stepHitEntity = candidate;
                    }
                    if (tempToi < distance &&
                        //If the hit is within a small margin range at the base of the character...
                        tempToi >= maximumStepHeight - body.collisionMargin - supportMargin && tempToi <= maximumStepHeight)
                    {
                        //Then this could be one of the non-step supports.
                        distance = tempToi;
                        hitLocation = tempHitLocation;
                        hitNormal = tempHitNormal;
                        hitEntity = candidate;
                    }
                }
            }
            Vector3 candidateLocationVelocity;
            if (stepDistance != float.MaxValue)
            {
                stepHitNormal.Normalize();
                candidateLocationVelocity = stepHitEntity.parent.internalLinearVelocity + //linear component
                                            Vector3.Cross(stepHitEntity.parent.internalAngularVelocity, stepHitLocation - stepHitEntity.parent.internalCenterPosition); //linear velocity of point on body relative to center

                //Analyze the hits.
                //Convert the 'times of impact' along the cast into distance by multiplying by the length of the cast.
                //Verify the "stepped" hit.  If it's invalid, go with the "non step" hit.
                if (stepDistance > 0 && //If hit distance was zero, it would probably mean that the outer convex cast found a wall (or something not steppable).
                    body.internalLinearVelocity.Y - candidateLocationVelocity.Y < glueSpeedToUse && //Don't try to 'glue' to the ground if we're just flying away from it.
                    isSupportSlopeWalkable(stepHitNormal) && //In order for this to be stepped on, it should be walkable.
                    (stepDistance > maximumStepHeight || isStepSafe(stepDistance))) //If its planning to step up, make sure its safe to do so.
                {
                    //Successfully found a stepping location.
                    supportEntity = stepHitEntity;
                    supportLocation = stepHitLocation;
                    supportNormal = stepHitNormal;
                    supportDistance = stepDistance;
                    supportLocationVelocity = candidateLocationVelocity;
                    return true;
                }
                else
                {
                    if (distance != float.MaxValue)
                    {
                        //Once it makes it here, it means there exists a support around the feet.
                        //Compute a new velocity for the feet support.
                        candidateLocationVelocity = hitEntity.parent.internalLinearVelocity + //linear component
                                                    Vector3.Cross(hitEntity.parent.internalAngularVelocity, hitLocation - hitEntity.parent.internalCenterPosition); //linear velocity of point on body relative to center

                        //The only condition that matters for a near-feet support is that the character is approaching the support rather than flying up away.
                        if (body.internalLinearVelocity.Y - candidateLocationVelocity.Y < glueSpeedToUse)
                        {

                            //While the step failed, there's still the backup of looking at the places right around the feet.
                            supportEntity = hitEntity;
                            supportLocation = hitLocation;
                            supportNormal = Vector3.Normalize(hitNormal);
                            supportDistance = distance;
                            supportLocationVelocity = candidateLocationVelocity;
                            return true;
                        }

                    }
                }
            }
            supportEntity = null;
            supportLocation = Toolbox.noVector;
            supportNormal = Toolbox.noVector;
            supportDistance = float.MaxValue;
            supportLocationVelocity = Toolbox.noVector;
            return false;
        }

        /// <summary>
        /// Finds the earliest hit between a convex cast and nearby geometry when using the given 
        /// </summary>
        /// <param name="convexCastShape">Shape of the convex object used in the convex cast.</param>
        /// <param name="startingLocation">The center position of the convexCastShape at the beginning of the convex cast.</param>
        /// <param name="direction">Normalized direction in which to send the convex cast.</param>
        /// <param name="maximumDistance">Longest distance that the convex cast will search before giving up.</param>
        /// <param name="supportEntity">First entity hit by the convex cast.</param>
        /// <param name="supportLocation">Location where the first entity was hit.</param>
        /// <param name="supportNormal">Normal at the location where the first entity was hit.</param>
        /// <param name="supportDistance">Distance from the maximum step height on the character down to the hit location.</param>
        /// <returns>Whether or not any hit location was found.</returns>
        bool getEarliestConvexCastHit(Entity convexCastShape, Vector3 startingLocation, Vector3 direction, float maximumDistance,
                                      out Entity earliestHitEntity, out Vector3 hitLocation, out Vector3 hitNormal, out float hitDistance)
        {
            hitDistance = float.MaxValue;
            earliestHitEntity = null;
            hitLocation = Toolbox.noVector;
            hitNormal = Toolbox.noVector;

            Vector3 sweep = new Vector3(0, -maximumDistance, 0);

            foreach (CollisionPair pair in feetCollisionPairCollector.collisionPairs)
            {
                Entity candidate;
                //Determine which member of the collision pair is the possible support.
                //The comparisons are all kept on a "parent" as opposed to "collider" level so that interaction with compound shapes is simpler.
                if (pair.parentA == feetCollisionPairCollector)
                    candidate = pair.parentB;
                else
                    candidate = pair.parentA;
                //Ensure that the candidate is a valid supporting entity.
                if (candidate.collisionRules.personal > CollisionRule.normal)
                    continue; //It is invalid!

                float toi;
                Vector3 tempHitLocation, tempHitNormal;

                //Fire a convex cast at the candidate and determine some details! 
                Vector3 centerPosition = candidate.internalCenterPosition;
                Quaternion orientation = candidate.internalOrientationQuaternion;
                if (Toolbox.areSweptObjectsColliding(convexCastShape, candidate,
                                                     ref startingLocation, ref centerPosition, ref Toolbox.identityOrientation, ref orientation,
                                                     0, candidate.collisionMargin, ref sweep, ref Toolbox.zeroVector, //Could use frame velocity as a candidate sweep, but not really important.
                                                     out tempHitLocation, out tempHitNormal, out toi))
                {
                    //We want to find the closest support, so compare it against the last closest support.
                    if (toi < hitDistance)
                    {
                        hitDistance = toi;
                        hitLocation = tempHitLocation;
                        earliestHitEntity = candidate;
                        if (toi > 0)
                            hitNormal = tempHitNormal;
                        else
                        {
                            hitNormal = Vector3.Up; //It seems that the ray started inside some object so there is no normal.  Use straight up!
                            //And since apparently the distance is 0, we can be reasonably confident that there is no 'closer' entity.
                            break;
                        }
                    }
                }
            }

            //Prior to this point, the hitDistance was actually a "time of impact" along the convex cast.
            //By multiplying by the length of the cast, the distance traveled can be found.
            hitDistance *= maximumDistance;
            return hitDistance < float.MaxValue;
        }

        /// <summary>
        /// Performs a convex cast to determine if a step of a given height is valid.
        /// This means that stepping up onto the new support wouldn't shove the character's head into a ceiling or other obstacle.
        /// </summary>
        /// <param name="hitDistance">Vertical distance from the convex cast start to the hit location.</param>
        /// <returns>Whether or not the step is safe.</returns>
        bool isStepSafe(float hitDistance)
        {
            float stepHeight = maximumStepHeight - hitDistance;
            Vector3 sweep = new Vector3(0, stepHeight + body.collisionMargin, 0);
            Vector3 startingLocation = headBlockageFinderOffset + body.internalCenterPosition;

            foreach (CollisionPair pair in headCollisionPairCollector.collisionPairs)
            {

                Entity candidate;
                //Determine which member of the collision pair is the possible blockage.
                //The comparisons are all kept on a "parent" as opposed to "collider" level so that interaction with compound shapes is simpler.

                if (pair.parentA == headCollisionPairCollector)
                    candidate = pair.parentB;
                else
                    candidate = pair.parentA;
                //Ensure that the candidate is a valid blocking entity.
                if (candidate.collisionRules.personal > CollisionRule.normal)
                    continue; //It is invalid!

                float toi;
                Vector3 tempHitLocation, tempHitNormal;

                //Fire a convex cast at the candidate and determine some details! 
                Vector3 centerPosition = candidate.internalCenterPosition;
                Quaternion orientation = candidate.internalOrientationQuaternion;
                if (Toolbox.areSweptObjectsColliding(castingShape, candidate,
                                                     ref startingLocation, ref centerPosition, ref Toolbox.identityOrientation, ref orientation,
                                                     0, candidate.collisionMargin, ref sweep, ref Toolbox.zeroVector, //Could use frame velocity as a candidate sweep, but not really important.
                                                     out tempHitLocation, out tempHitNormal, out toi))
                {
                    return false;
                }
            }
            return true;
        }

        /// <summary>
        /// Determines if the ground supporting the character is sloped gently enough to allow for normal walking.
        /// </summary>
        /// <param name="supportNormal">Normal of the surface being stood upon.</param>
        /// <returns>Whether or not the slope is walkable.</returns>
        bool isSupportSlopeWalkable(Vector3 supportNormal)
        {
            //The following operation is equivalent to performing a dot product between the support normal and Vector3.Down and finding the angle it represents using Acos.
            return Math.Acos(Math.Abs(Math.Min(supportNormal.Y, 1))) <= maxSlope;

        }

        /// <summary>
        /// Maintains the position of the character's body above the ground.
        /// </summary>
        /// <param name="supportEntity">The closest supporting entity.</param>
        /// <param name="supportLocation">The support location where the ray hit the entity.</param>
        /// <param name="supportLocationVelocity">Velocity of the support point connected to the supportEntity.</param>
        /// <param name="supportNormal">The normal at the surface where the ray hit the entity.</param>
        /// <param name="supportDistance">Distance from the character to the support location.</param>
        void support(Entity supportEntity, Vector3 supportLocation, Vector3 supportLocationVelocity, Vector3 supportNormal, float supportDistance)
        {
            //Put the character at the right distance from the ground.
            float heightDifference = maximumStepHeight - supportDistance;
            body.teleport(new Vector3(0, heightDifference, 0));

            //Remove from the character velocity which would push it toward or away from the surface.
            //This is a relative velocity, so the velocity of the body and the velocity of a point on the support entity must be found.
            float bodyNormalVelocity = Vector3.Dot(body.internalLinearVelocity, supportNormal);
            float supportEntityNormalVelocity = Vector3.Dot(supportLocationVelocity, supportNormal);
            body.internalLinearVelocity -= (bodyNormalVelocity - supportEntityNormalVelocity) * supportNormal;

        }

        /// <summary>
        /// Manages movement acceleration, deceleration, and sliding.
        /// </summary>
        /// <param name="supportEntity">The closest supporting entity.</param>
        /// <param name="supportLocation">The support location where the ray hit the entity.</param>
        /// <param name="supportLocationVelocity">Velocity of the support point connected to the supportEntity.</param>
        /// <param name="supportNormal">The normal at the surface where the ray hit the entity.</param>
        /// <param name="supportDistance">Distance from the character to the support location.</param>
        /// <param name="dt">Timestep of the simulation.</param>
        void handleHorizontalMotion(Entity supportEntity, Vector3 supportLocation, Vector3 supportLocationVelocity, Vector3 supportNormal, float supportDistance, float dt)
        {


            if (hasTraction && movementDirection != Vector2.Zero)
            {
                //Identify a coordinate system that uses the support normal as Y.
                //X is the axis point along the left (negative) and right (positive) relative to the movement direction.
                //Z points forward (positive) and backward (negative) in the movement direction modified to be parallel to the surface.
                Vector3 x = Vector3.Cross(new Vector3(movementDirection.X, 0, movementDirection.Y), supportNormal);
                Vector3 z = Vector3.Cross(supportNormal, x);

                //Remove from the character a portion of velocity which pushes it horizontally off the desired movement track defined by the movementDirection.
                float bodyXVelocity = Vector3.Dot(body.internalLinearVelocity, x);
                float supportEntityXVelocity = Vector3.Dot(supportLocationVelocity, x);
                float velocityChange = MathHelper.Clamp(bodyXVelocity - supportEntityXVelocity, -dt * tractionDeceleration, dt * tractionDeceleration);
                body.internalLinearVelocity -= velocityChange * x;


                float bodyZVelocity = Vector3.Dot(body.internalLinearVelocity, z);
                float supportEntityZVelocity = Vector3.Dot(supportLocationVelocity, z);
                float netZVelocity = bodyZVelocity - supportEntityZVelocity;
                //The velocity difference along the Z axis should accelerate/decelerate to match the goal velocity (max speed).
                if (netZVelocity > maxSpeed)
                {//Decelerate
                    velocityChange = Math.Min(dt * tractionDeceleration, netZVelocity - maxSpeed);
                    body.internalLinearVelocity -= velocityChange * z;
                }
                else
                {//Accelerate
                    velocityChange = Math.Min(dt * acceleration, maxSpeed - netZVelocity);
                    body.internalLinearVelocity += velocityChange * z;
                }

            }
            else
            {
                float deceleration;
                if (hasTraction)
                    deceleration = dt * tractionDeceleration;
                else
                    deceleration = dt * slidingDeceleration;
                //Remove from the character a portion of velocity defined by the deceleration.
                Vector3 bodyHorizontalVelocity = body.internalLinearVelocity - Vector3.Dot(body.internalLinearVelocity, supportNormal) * supportNormal;
                Vector3 supportHorizontalVelocity = supportLocationVelocity - Vector3.Dot(supportLocationVelocity, supportNormal) * supportNormal;

                Vector3 relativeVelocity = bodyHorizontalVelocity - supportHorizontalVelocity;
                float speed = relativeVelocity.Length();
                Vector3 horizontalDirection;
                if (speed > 0)
                {
                    horizontalDirection = relativeVelocity / speed;
                    float velocityChange = Math.Min(speed, deceleration);
                    body.internalLinearVelocity -= velocityChange * horizontalDirection;
                }



            }




        }

        /// <summary>
        /// Manages the character's air control.
        /// </summary>
        /// <param name="dt">Timestep of the simulation.</param>
        void handleAirborneMotion(float dt)
        {
            //Compute the current horizontal speed, two dimensional dot product.
            float speed = body.internalLinearVelocity.X * movementDirection.X + body.internalLinearVelocity.Z * movementDirection.Y;

            float previousSpeed = speed;
            speed = Math.Min(maximumAirborneSpeed, speed + airborneAcceleration * dt);
            float changeInSpeed = MathHelper.Max(0, speed - previousSpeed);

            body.internalLinearVelocity += new Vector3(changeInSpeed * movementDirection.X, 0, changeInSpeed * movementDirection.Y);
        }

        /// <summary>
        /// If the character has a support, it leaps into the air based on its jumpSpeed.
        /// </summary>
        public void jump()
        {
            if (isSupported)
            {
                isSupported = false;
                hasTraction = false;
                body.internalLinearVelocity += new Vector3(0, jumpSpeed, 0);
            }
        }

        /// <summary>
        /// Activates the character, adding its components to the space. 
        /// </summary>
        public void activate()
        {
            if (!isUpdating)
            {
                isUpdating = true;
                if (body.space == null)
                {
                    mySpace.add(body);
                    mySpace.add(feetCollisionPairCollector);
                    mySpace.add(headCollisionPairCollector);
                }
                hasTraction = false;
                isSupported = false;
                body.internalLinearVelocity = Vector3.Zero;
            }
        }

        /// <summary>
        /// Deactivates the character, removing its components from the space.
        /// </summary>
        public void deactivate()
        {
            if (isUpdating)
            {
                isUpdating = false;
                body.teleportTo(new Vector3(10000, 0, 0));
                if (body.space != null)
                {
                    mySpace.remove(body);
                    mySpace.remove(feetCollisionPairCollector);
                    mySpace.remove(headCollisionPairCollector);
                }

            }
        }

        /// <summary>
        /// Called by the engine when the character is added to the space.
        /// Activates the character.
        /// </summary>
        /// <param name="newSpace">Space to which the character was added.</param>
        public override void onAdditionToSpace(Space newSpace)
        {
            activate();
        }

        /// <summary>
        /// Called by the engine when the character is removed from the space.
        /// Deactivates the character.
        /// </summary>
        public override void onRemovalFromSpace()
        {
            deactivate();
        }
    }
}