GoakeFlax/Source/Game/PlayerMovement.cs

using System;
using System.Collections.Generic;
using FlaxEngine;
using System.Diagnostics;
using System.Threading.Tasks;
using Console = Cabrito.Console;
using Debug = FlaxEngine.Debug;
using Object = FlaxEngine.Object;

namespace Game
{
	public struct TraceInfo
	{
		public RayCastHit[] hitInfos;
		public bool startSolid;

		// closest hit
		public float fraction;
		public Vector3 endPosition;
		public Vector3 hitNormal;
		public Vector3 hitPosition;

		// furthest hit
		//public float maxFraction;
		//public Vector3 maxHitNormal;
		//public Vector3 maxEndPosition;
	}

	public class PlayerMovement : Script
	{
		[Limit(0, 9000), Tooltip("Base Movement speed")]
		public float MoveSpeed { get; set; } = 320;

		private static Vector3 Gravity { get; set; } = new Vector3(0, -800.0f, 0f);

		private float viewPitch;
		private float viewYaw;
		private float viewRoll;

		private float viewPitchLastFrame;
		private float viewYawLastFrame;
		private float viewRollLastFrame;

		private bool predicting = false;

		private InputEvent onExit = new InputEvent("Exit");

		// FIXME, should be much smaller but needed to avoid issues with box collider edges against brush edges diagonally
		private const float collisionMargin = 0.031f * 1.666f * 1.85f;
		private const float slopeNormal = 0.7f;

		private Actor rootActor;
		private RigidBody rigidBody;
		private PlayerInput input;

		public override void OnAwake()
		{
			base.OnAwake();

			bool record = false;
			//record = true;

			if (record)
			{
				input = new PlayerInputLocal(@"C:\dev\GoakeFlax\testdemo.gdem"); // record
			}
			else
			{
				input = new PlayerInputLocal();
				//input = new PlayerInputDemo(@"C:\dev\GoakeFlax\testdemo.gdem"); //playback
				//input = new PlayerInputDemo(@"C:\dev\GoakeFlax\testdemo_desync.gdem"); //playback
			}

			onExit.Triggered += () =>
			{
				if (Console.IsSafeToQuit)
					Engine.RequestExit();
			};

			rootActor = Actor.GetChild(0);

			rigidBody = Actor.As<RigidBody>();

			//rigidBody.CollisionEnter += OnCollisionEnter;
			//rigidBody.TriggerEnter += OnTriggerEnter;
			//rigidBody.TriggerExit += OnTriggerExit;

			startupTime = Time.TimeSinceStartup;
		}

		public override void OnDisable()
		{
			base.OnDisable();

			if (input != null && input is PlayerInputLocal) // FIXME
				(input as PlayerInputLocal).StopRecording();
		}

		private List<PhysicsColliderActor> touchingActors = new List<PhysicsColliderActor>();

		private void OnTriggerEnter(PhysicsColliderActor colliderActor)
		{
			//if (colliderActor.AttachedRigidBody == null)
			//    return;
			touchingActors.Add(colliderActor);
			Console.Print("trogger: ");
		}

		private void OnTriggerExit(PhysicsColliderActor colliderActor)
		{
			//if (colliderActor.AttachedRigidBody == null)
			//    return;

			touchingActors.Remove(colliderActor);
			Console.Print("untrogger: ");
		}

		private void OnCollisionEnter(Collision collision)
		{
			//Console.Print("collision: ");
		}

		public override void OnDestroy()
		{
			base.OnDestroy();

			onExit.Dispose();
		}

		public override void OnStart()
		{
			var initialEulerAngles = Actor.Orientation.EulerAngles;
			viewPitch = initialEulerAngles.X;
			viewYaw = initialEulerAngles.Y;
			viewRoll = initialEulerAngles.Z;
			viewPitchLastFrame = viewPitch;
			viewYawLastFrame = viewYaw;
			viewRollLastFrame = viewRoll;
		}

		private int lastInputFrame = 0;
		private int currentInputFrame = 0;
		private int currentInputFrame2 = 0;
		private float startupTime = 0f;
		public override void OnUpdate()
		{
			//input.OnUpdate();

			if (input is PlayerInputDemo /*&& currentInputFrame2 >= currentInputFrame*/)
				return;

			input.OnUpdate();

			/*if (input.frame > 0)
			{
				PlayerActorState actorState = input.GetCurrentActorState();
				Actor.Position = actorState.position;
				currentVelocity = actorState.velocity;
				viewYaw = actorState.viewYaw;
				viewPitch = actorState.viewPitch;
				viewRoll = actorState.viewRoll;
			}*/

			PlayerInputState inputState = input.GetCurrentInputState();

			viewYaw = viewYawLastFrame;
			viewPitch = viewPitchLastFrame;
			viewRoll = viewRollLastFrame;
			ApplyInputToCamera(inputState);

			/*input.RecordCurrentActorState(new PlayerActorState()
			{
				position = Actor.Position,
				velocity = currentVelocity,
				orientation = rootActor.Orientation,
				viewYaw = viewYaw,
				viewPitch = viewPitch,
				viewRoll = viewRoll
			});*/
			currentInputFrame2++;
		}

		private bool demoDeltasVerify = true;
		private bool demoDeltasCorrect = true;

		public override void OnFixedUpdate()
		{
			if (input is PlayerInputDemo)
				input.OnUpdate();

			float deltadif = Time.DeltaTime - (1.0f / Time.PhysicsFPS);
			if (Math.Abs(deltadif) > 0.0001f)
				Console.Print("drift: " + deltadif);

			input.OnFixedUpdate();
			PlayerInputState inputState = input.GetCurrentInputState();

			if (input is PlayerInputDemo)
			{
				ApplyInputToCamera(inputState);

				// Verify view angles first
				if (demoDeltasVerify)
				{
					Vector3 viewAngles = new Vector3(viewYaw, viewPitch, viewRoll);
					float viewAnglesDelta = (viewAngles - inputState.verificationViewAngles).Length;
					if (viewAnglesDelta > 0.00001)
					{
						Console.PrintError("Demo verification failed, view angles delta: " + viewAnglesDelta);
						if (demoDeltasCorrect)
						{
							SetCameraEulerAngles(inputState.verificationViewAngles);
						}
					}
				}
			}

			SimulatePlayerMovement(inputState);

			if (input is PlayerInputDemo && demoDeltasVerify)
			{
				// verify
				float positionDelta = (Actor.Position - inputState.verificationPosition).Length;
				if (positionDelta > 0.00001)
					Console.Print("Demo verification failed, position delta: " + positionDelta);

				float velocityDelta = (currentVelocity - inputState.verificationVelocity).Length;
				if (velocityDelta > 0.00001)
					Console.Print("Demo verification failed, velocity delta: " + velocityDelta);

				float orientationDelta = (rootActor.Orientation - inputState.verificationOrientation).Length;
				if (orientationDelta > 0.00001)
				{
					Console.PrintError("Demo verification failed, orientation delta: " + orientationDelta);
					if (demoDeltasCorrect)
					{

					}
				}


				//if (currentInputFrame == 0)
				/*{
					//Console.Print("repos: " + inputState.verificationPosition);
					Actor.Position = inputState.verificationPosition;
					currentVelocity = inputState.verificationVelocity;
					rootActor.Orientation = inputState.verificationOrientation;
				}*/
			}

			input.RecordCurrentActorState(new PlayerActorState()
			{
				position = Actor.Position,
				velocity = currentVelocity,
				orientation = rootActor.Orientation,
				viewAngles = new Vector3(viewYaw, viewPitch, viewRoll),
			});
			input.OnEndFrame();


			lastInputFrame = currentInputFrame;
			currentInputFrame++;

			viewPitchLastFrame = viewPitch;
			viewYawLastFrame = viewYaw;
			viewRollLastFrame = viewRoll;
		}

		private void ApplyInputToCamera(PlayerInputState inputState)
		{
			// Update camera viewf
			float xAxis = inputState.viewDeltaX;
			float yAxis = inputState.viewDeltaY;
			if (xAxis == 0.0f && yAxis == 0.0f)
				return;

			viewPitch = Mathf.Clamp(viewPitch + yAxis, -90.0f, 90.0f);
			viewYaw += xAxis;

			SetCameraEulerAngles(new Vector3(viewYaw, viewPitch, viewRoll));
		}

		private void SetCameraEulerAngles(Vector3 viewAngles)
		{
			//Camera camera = rootActor.GetChild<Camera>();
			Actor cameraHolder = rootActor.GetChild("CameraHolder");

			// Root orientation must be set first
			rootActor.Orientation = Quaternion.Euler(0, viewAngles.X, 0);
			cameraHolder.Orientation = Quaternion.Euler(viewAngles.Y, viewAngles.X, viewAngles.Z);
		}

		private static bool SweepPlayerCollider(Actor actor, Vector3 start, Vector3 end, out RayCastHit[] hits)
		{
			Vector3 delta = end - start;
			float maxDistance = delta.Length;
			Vector3 direction = delta.Normalized;

			bool collided = false;
			var capsuleCollider = actor.GetChild<CapsuleCollider>();
			var boxCollider = actor.GetChild<BoxCollider>();
			var meshCollider = actor.GetChild<MeshCollider>();
			if (capsuleCollider && capsuleCollider.IsActive)
			{
				collided = Physics.CapsuleCastAll(start,
					capsuleCollider.Radius, capsuleCollider.Height,
					direction, out hits, capsuleCollider.Orientation, maxDistance,
					uint.MaxValue,
					false);
			}
			else if (meshCollider && meshCollider.IsActive)
			{
				collided = Physics.ConvexCastAll(start,
					meshCollider.CollisionData, meshCollider.Scale,
					direction, out hits, meshCollider.Orientation, maxDistance,
					uint.MaxValue,
					false);
			}
			else if (boxCollider && boxCollider.IsActive)
			{
				collided = Physics.BoxCastAll(start,
					boxCollider.OrientedBox.Extents,
					direction, out hits, boxCollider.Orientation, maxDistance,
					uint.MaxValue,
					false);
			}
			else
			{
				throw new Exception("Player does not have a collider");
			}

			return collided;
		}

		private bool SweepPlayerCollider(Vector3 position, out PhysicsColliderActor[] colliders)
		{
			bool collided = false;
			var capsuleCollider = Actor.GetChild<CapsuleCollider>();
			var boxCollider = Actor.GetChild<BoxCollider>();
			var meshCollider = Actor.GetChild<MeshCollider>();
			PhysicsColliderActor colliderActor = null;
			if (capsuleCollider && capsuleCollider.IsActive)
			{
				colliderActor = capsuleCollider;
				collided = Physics.OverlapCapsule(position,
					capsuleCollider.Radius, capsuleCollider.Height,
					out colliders, capsuleCollider.Orientation,
					uint.MaxValue,
					false);
			}
			else if (meshCollider && meshCollider.IsActive)
			{
				colliderActor = meshCollider;
				collided = Physics.OverlapConvex(position,
					meshCollider.CollisionData, meshCollider.Scale,
					out colliders, meshCollider.Orientation,
					uint.MaxValue,
					false);
			}
			else if (boxCollider && boxCollider.IsActive)
			{
				colliderActor = boxCollider;
				collided = Physics.OverlapBox(position,
					boxCollider.OrientedBox.Extents,
					out colliders, boxCollider.Orientation,
					uint.MaxValue,
					false);
			}
			else
			{
				throw new Exception("Player does not have a collider");
			}

			if (collided)
			{
				List<PhysicsColliderActor> collidersFiltered = new List<PhysicsColliderActor>();
				foreach (var collider in colliders)
				{
					if (collider.Parent == Actor)
						continue;

					collidersFiltered.Add(collider);
				}

				colliders = collidersFiltered.ToArray();
				if (colliders.Length == 0)
					collided = false;
			}

			return collided;
		}

		/// <summary>
		/// Sweeps the player rigidbody in world and returns geometry which was hit during the trace.
		/// </summary>
		/// <param name="actor">Player actor</param>
		/// <param name="start">Start position</param>
		/// <param name="end">End position</param>
		/// <returns></returns>
		private static TraceInfo TracePlayer(Actor actor, Vector3 start, Vector3 end)
		{
			TraceInfo traceInfo = new TraceInfo();

			Vector3 delta = end - start;
			float maxDistance = delta.Length;
			Vector3 direction = delta.Normalized;

			bool collided = SweepPlayerCollider(actor, start, end, out traceInfo.hitInfos);
			if (collided)
			{
				List<RayCastHit> hitInfosFiltered = new List<RayCastHit>();
				RayCastHit closest = new RayCastHit();
				closest.Distance = float.MaxValue;
				foreach (var hitInfo in traceInfo.hitInfos)
				{
					//if (hitInfo.Collider == colliderActor)
					//	continue;
					if (hitInfo.Collider.Parent == actor)
						continue;

					hitInfosFiltered.Add(hitInfo);

					if (hitInfo.Distance < closest.Distance && hitInfo.Distance != 0.0f)
						closest = hitInfo;
				}

				if (hitInfosFiltered.Count == 0 /*|| closest.Distance == float.MaxValue*/)
					collided = false; // self-collision?
				/*else if (closest.Distance == float.MaxValue)
				{
					bool startSolid = SweepPlayerCollider(start, out PhysicsColliderActor[] colliders);
					if (startSolid)
					{
						traceInfo.hitInfos = hitInfosFiltered.ToArray();

						traceInfo.fraction = 0f;
						traceInfo.hitNormal = Vector3.Zero;
						traceInfo.hitPosition = start;
						traceInfo.endPosition = start;
						traceInfo.startSolid = true;

						if (delta.Y >= 0f)
							Console.Print("ovr: " + colliders[0].Parent.Name);
					}
					else
						collided = false;
				}*/
				else //if (closest.Distance > 0f)
				{
					if (closest.Distance == float.MaxValue)
					{
						foreach (var hitInfo in hitInfosFiltered)
						{
							if (hitInfo.Distance < closest.Distance)
								closest = hitInfo;
						}
					}

					traceInfo.hitInfos = hitInfosFiltered.ToArray();

					traceInfo.fraction = closest.Distance / maxDistance;
					traceInfo.hitNormal = closest.Normal;
					traceInfo.hitPosition = closest.Point;
					traceInfo.endPosition = start + (delta * traceInfo.fraction);

					if (traceInfo.fraction == 0f && maxDistance > 0f)
						traceInfo.startSolid = true;

					//if (delta.Y >= 0f)
					//	Console.Print("col: " + closest.Collider.Parent.Name + ", " + closest.Distance.ToString("G9"));
				}
				/*else
				{
				    traceInfo.startSolid = true;
				    traceInfo.fraction = 0f;
				}*/
			}

			if (!collided)
			{
				traceInfo.hitInfos = new RayCastHit[0];
				traceInfo.fraction = 1f;
				traceInfo.endPosition = end;
			}

			return traceInfo;
		}

#if FLAX_EDITOR
		public override void OnDebugDraw()
		{
			base.OnDebugDraw();

			var capsuleCollider = Actor.GetChild<CapsuleCollider>();
			var boxCollider = Actor.GetChild<BoxCollider>();
			var meshCollider = Actor.GetChild<MeshCollider>();
			if (capsuleCollider && capsuleCollider.IsActive)
			{
				Quaternion rotation = capsuleCollider.LocalOrientation * Quaternion.Euler(0f, 90f, 0f);
				DebugDraw.DrawWireTube(capsuleCollider.Position, rotation, capsuleCollider.Radius,
					capsuleCollider.Height, Color.GreenYellow * 0.8f);
			}
			else if (meshCollider && meshCollider.IsActive)
			{
				//Quaternion rotation = meshCollider.LocalOrientation * Quaternion.Euler(0f, 90f, 0f);
				DebugDraw.DrawWireCylinder(meshCollider.Position, meshCollider.Orientation, capsuleCollider.Radius,
					capsuleCollider.Height + capsuleCollider.Radius * 2, Color.GreenYellow * 0.8f);
				//DebugDraw.DrawWireTube(meshCollider.Position, rotation, meshCollider.Radius, meshCollider.Height, Color.GreenYellow * 0.8f);
			}
			else if (boxCollider && boxCollider.IsActive)
			{
				DebugDraw.DrawWireBox(boxCollider.OrientedBox.GetBoundingBox(), Color.GreenYellow * 0.8f);
			}
		}
#endif

		private static SlideMoveHit StepSlideMove(Actor actor, ref Vector3 position, ref Vector3 velocity, bool onGround)
		{
			if (velocity.IsZero)
				return SlideMoveHit.Nothing;

			Vector3 gravityDirection = Gravity.Normalized;

			Vector3 originalPosition = position;
			Vector3 originalVelocity = velocity;

			SlideMoveHit slideMoveHit = SlideMove(actor, ref position, ref velocity);
			if (slideMoveHit == SlideMoveHit.Nothing)
			{
				// TODO: step down here
				return slideMoveHit;
			}

			// hit something, try to step up
			float effectiveStepHeight = stepHeight;
			if (!onGround)
			{
				// TODO: implement clipping here
				/*if (pmove.jump_time > 0 && pmove.jump_time <= movevars.cliptime)
				{
					float zvel = pmove.velocity[2];
					VectorCopy(originalvel, pmove.velocity);
					pmove.velocity[2] = min(pmove.velocity[2], zvel); // nullifies vertical clipping
				}*/

				if (!slideMoveHit.HasFlag(SlideMoveHit.Step))
					return slideMoveHit;

				if (airStep < 2)
				{
					//effectiveStepHeight = ?
				}
			}


			Vector3 stepDelta = -gravityDirection * effectiveStepHeight;

			Vector3 slidePosition = position;
			Vector3 slideVelocity = velocity;
			position = originalPosition;
			velocity = originalVelocity;

			// step up
			Vector3 stepUp = position + stepDelta;
			TraceInfo traceUp = TracePlayer(actor, position, stepUp);
			if (traceUp.fraction > 0f)
				position = traceUp.endPosition;

			// try moving from step up position
			SlideMoveHit slideMoveStepHit = SlideMove(actor, ref position, ref velocity);

			// step down
			Vector3 stepDown = position - stepDelta;
			TraceInfo traceDown = TracePlayer(actor, position, stepDown);
			if (traceDown.fraction < 1f && -Vector3.Dot(gravityDirection, traceDown.hitNormal) < slopeNormal)
			{
				// can't step down, slide move like normally
				Console.Print("no stepping 1, frac: " + traceDown.fraction + ", dot: " +
				              (-Vector3.Dot(gravityDirection, traceDown.hitNormal)) +
				              ", norm: " + traceDown.hitNormal);
				position = slidePosition;
				velocity = slideVelocity;
				return slideMoveHit;
			}
			else if (traceDown.fraction > 0f)
				position = traceDown.endPosition;

			// add some margin from the ground in order to avoid getting stuck after stepping up
			if (traceDown.fraction < 1f)
				position.Y += collisionMargin;

			// ??
			var d1 = -Vector3.Dot(gravityDirection, position);
			var d2 = -Vector3.Dot(gravityDirection, originalPosition);
			if (d1 < d2)
			{
				//Console.Print("no stepping 2, " + d1 + " < " + d2);
				position = slidePosition;
				velocity = slideVelocity;
				return slideMoveHit;
			}

			Vector3 slidePosition2 = slidePosition; //down
			Vector3 stepPosition2 = position; //up

			// FIXME, negate gravity
			slidePosition2.Y = 0f;
			stepPosition2.Y = 0f;

			// take the slide movement results if furthest away from original position
			//if ((stepPosition2 - originalPosition).Length < (slidePosition2 - originalPosition).Length)
			if ((slidePosition2 - originalPosition).Length >= (stepPosition2 - originalPosition).Length)
			{
				//Console.Print("no stepping 3");
				position = slidePosition;
				velocity = slideVelocity;
				return slideMoveHit;
			}

			return slideMoveStepHit;
		}

		[Flags]
		enum SlideMoveHit
		{
			Nothing = 0,
			Step = 1,
			Floor = 2,
			Other = 4,
		}

		private static SlideMoveHit SlideMove(Actor actor, ref Vector3 position, ref Vector3 velocity)
		{
			if (velocity.IsZero)
				return SlideMoveHit.Nothing;

			Vector3 originalPosition = position;
			Vector3 originalVelocity = velocity;
			SlideMoveHit slideMoveHit = SlideMoveHit.Nothing;

			float timeleft = Time.DeltaTime;

			List<Vector3> hitNormals = new List<Vector3>();

			for (int bump = 0; bump < 4; bump++)
			{
				Vector3 startPos = position;
				Vector3 endPos = position + (velocity * timeleft);

				TraceInfo trace = TracePlayer(actor, startPos, endPos);
				// TODO: handle portals here

				float fraction = trace.fraction;
				Vector3 hitNormal = trace.hitNormal;

				if (trace.startSolid)
				{
					velocity = Vector3.Zero;
					break;
				}

				if (fraction > 0f)
				{
					position = trace.endPosition;
					hitNormals.Clear(); // this is present in some forks, not in Q3
				}

				if (fraction >= 1f)
					break;

				timeleft *= 1.0f - fraction;

				if (trace.hitNormal.Y > slopeNormal)
					slideMoveHit |= SlideMoveHit.Floor;
				else if (Math.Abs(trace.hitNormal.Y) < 0.0001f)
					slideMoveHit |= SlideMoveHit.Step;
				else
					slideMoveHit |= SlideMoveHit.Other;

				// this doesn't seem to do anything, we never have any hitNormals stored here
				bool hitPreviousNormal = false;
				foreach (Vector3 normal in hitNormals)
				{
					if (Vector3.Dot(hitNormal, normal) > 0.99)
					{
						// nudge away from the same wall we hit earlier and try again
						velocity += hitNormal;
						hitPreviousNormal = true;
						break;
					}
				}

				if (hitPreviousNormal)
					continue;

				hitNormals.Add(hitNormal);
				if (hitNormals.Count != 1)
					Console.Print("hitNormals: " + hitNormals.Count);

				int plane;
				Vector3 normalMargin = Vector3.Zero;
				for (plane = 0; plane < hitNormals.Count; plane++)
				{
					Vector3 normal = hitNormals[plane];

					// clip velocity
					velocity -= normal * Vector3.Dot(velocity, normal);
					//velocity = Vector3.ProjectOnPlane(velocity, normal);

					//traceOffset = normal * 1f;
					normalMargin += normal;
					//position += normal * 0.031f;

					int plane2;
					for (plane2 = 0; plane2 < hitNormals.Count; plane2++)
					{
						if (plane == plane2)
							continue;

						if (Vector3.Dot(velocity, hitNormals[plane2]) < 0f)
							break;
					}

					if (plane2 == hitNormals.Count)
						break;
				}

				// push off slightly away from the walls to not get stuck
				position += normalMargin.Normalized * collisionMargin;
				//Console.Print("pushin");

				if (plane == hitNormals.Count)
				{
					if (hitNormals.Count == 2)
					{
						Vector3 dir = Vector3.Cross(hitNormals[0], hitNormals[1]);
						//dir.Normalize();
						float dist = Vector3.Dot(dir, velocity);
						velocity = dist * dir;
					}
					else
					{
						velocity = Vector3.Zero;
						break;
					}
				}
				else
				{
					// nudge very slightly away from the wall to avoid getting stuck
					//position += trace.hitNormal * 0.01f;
					//velocity += trace.hitNormal * 0.01f;
				}

				// prevents bouncing against the wall
				if ( /*velocity.Length > 0f && */Vector3.Dot(velocity, originalVelocity) <= 0f)
				{
					velocity = Vector3.Zero;
					break;
				}
			}

			return slideMoveHit;
		}

		[ReadOnly] public bool onGround = false;

		/*
		// QW
		private const float friction = 4f;
		private const float stopspeed = 100f;
		private const float accelerationGround = 10f;
		private const float jumpVelocity = 270f;

		private const float maxAirSpeed = 320f;
		private const float maxAirStrafeSpeed = 30f; //Q2+
		private const float airAcceleration = 0f; //Q2+
		private const float airStopAcceleration = 0f; //Q2+
		private const float airStrafeAcceleration = 0f; //CPM?
		private const float strafeAcceleration = 10f; //QW
		private const float airControl = 0f; //CPM
		private const float stepSize = 16f;
		private const float autoJumpTime = 0.4f;

		private const int airStep = 0;
		*/

		// GOA
		private const float friction = 6f;
		private const float stopspeed = 100f;
		private const float accelerationGround = 12f;
		private const float jumpVelocity = 270f;

		private const float maxAirSpeed = 320f;
		private const float maxAirStrafeSpeed = 30f;
		private const float airAcceleration = 0.4f;
		private const float airStopAcceleration = 2.5f;
		private const float airStrafeAcceleration = 0f;
		private const float strafeAcceleration = 10f;
		private const float airControl = 0f;
		private const float stepHeight = 16f;
		private const float autoJumpTime = 0.4f;

		private const int airStep = 2;

		//private bool physicsInteractions = false;

		private bool jumped = false;
		private float lastJumped = -1f;
		private float lastLanded = -1f;

		//private Vector3 safePosition;

		[ReadOnly]
		public float CurrentVelocity
		{
			get { return currentVelocity.Length; }
			set { }
		}

		[ReadOnly]
		public float UPS
		{
			get
			{
				Vector3 horizontalSpeed = currentVelocity;
				horizontalSpeed.Y = 0f;
				return horizontalSpeed.Length;
			}
			set { }
		}

		private Vector3 currentVelocity;

		public void SimulatePlayerMovement(PlayerInputState inputState)
		{
			Transform rootTrans = rootActor.Transform;

			Vector3 inputDirection =
				new Vector3(inputState.moveRight, 0.0f, inputState.moveForward);
			Vector3 moveDirection = rootTrans.TransformDirection(inputDirection);

			Vector3 position = rigidBody.Position;
			Vector3 velocity = currentVelocity; //rigidBody.LinearVelocity;

			Vector3 wishVelocity = Vector3.Zero;
			if (!inputDirection.IsZero)
				wishVelocity = moveDirection.Normalized * MoveSpeed;

			// categorize position
			bool lastGround = onGround;
			Vector3 lastVelocity = velocity;
			onGround = true;
			Vector3 groundDelta = Gravity.Normalized;//Gravity.Normalized * (collisionMargin * 2);
			//if (velocity.Y < 0f)
			//	groundDelta = Gravity.Normalized * velocity.Y * Time.DeltaTime;
			TraceInfo traceGround = TracePlayer(Actor, position, position + groundDelta);
			//Console.PrintDebug(1, true, "startSolid: " + traceGround.startSolid);
			if (!traceGround.startSolid && traceGround.fraction < 1f &&
			    -Vector3.Dot(Gravity.Normalized, traceGround.hitNormal) < slopeNormal)
			{
				//Console.Print("slope?");
				// slope
				// clip velocity
				Vector3 bounce = groundDelta;
				//Vector3 velocityProjected = Vector3.ProjectOnPlane(velocity, normal);
				float backoff = Vector3.Dot(bounce, traceGround.hitNormal) * 2f;
				bounce -= traceGround.hitNormal * backoff;
				//velocity = velocityProjected;

				Vector3 point = (position + groundDelta) +
				                (1f - traceGround.fraction) * bounce;

				Console.Print("backoff: " + backoff);

				// retrace
				traceGround = TracePlayer(Actor, position, position + point);
			}

			if (!traceGround.startSolid && (traceGround.fraction >= 1f ||
			                                -Vector3.Dot(Gravity.Normalized, traceGround.hitNormal) < slopeNormal))
			{
				// falling or sliding down a slope
				onGround = false;
				//Console.PrintDebug(1, true, "fall or slide");
			}
			else
			{
				//if (onGround != !traceGround.startSolid)
				//	Console.Print("slidefrac: " + traceGround.fraction);
				onGround = !traceGround.startSolid;
				//Console.PrintDebug(1, true, "issolid? :" + traceGround.startSolid);
			}

			// TODO: snap to ground here

			bool jumpAction = inputState.jumping;

			if (jumped && !jumpAction)
				jumped = false; // jump released
			else if (jumped && Time.GameTime - lastJumped >= autoJumpTime)
				jumped = false; // jump timeout

			// jump
			if (onGround && jumpAction && !jumped)
			{
				if (OnJump(traceGround, ref velocity))
				{
					onGround = false;
					jumped = true;
					lastJumped = Time.GameTime;
				}
			}

			//if (/*onGround && */lastGround != onGround)
			if (onGround)
			{
				// ground friction
				ApplyFriction(ref velocity);

				// ground acceleration
				ApplyAcceleration(ref velocity, wishVelocity.Normalized, wishVelocity.Length, float.MaxValue,
					accelerationGround);
			}
			else // air movement
			{
				ApplyAirAcceleration(ref velocity, wishVelocity);
			}

			StepSlideMove(Actor, ref position, ref velocity, onGround);

			rigidBody.Position = position;
			currentVelocity = velocity;
			//rigidBody.LinearVelocity = velocity;

			const float landingVelocityThreshold = 120f;
			const float landingHardVelocityThreshold = 500f;
			if (currentVelocity.Y - lastVelocity.Y > landingVelocityThreshold)
			{
				if (Time.GameTime - lastJumped > 0.01)
				{
					bool hardLanding = currentVelocity.Y - lastVelocity.Y > landingHardVelocityThreshold;
					OnLanded(currentVelocity - lastVelocity, hardLanding);
					lastLanded = Time.GameTime;
				}
			}
		}

		private bool OnJump(TraceInfo traceGround, ref Vector3 velocity)
		{
			// reset velocity from gravity
			if (-Vector3.Dot(Gravity.Normalized, velocity) < 0 &&
			    Vector3.Dot(velocity, traceGround.hitNormal) < -0.1)
			{
				velocity = Vector3.ProjectOnPlane(velocity, traceGround.hitNormal);
			}

			velocity += Vector3.Up * jumpVelocity;

			if (!predicting)
			{
				// Avoid overlapping with recent landing sound
				if (Time.GameTime - lastLanded > 0.3)
					AudioManager.PlaySound("jumpland", Actor, 0, rootActor.Position, 1f /*, new Vector2(0.7f, 1.3f)*/);
			}

			return true;
		}

		private void OnLanded(Vector3 landingVelocity, bool hardLanding)
		{
			if (!predicting)
				AudioManager.PlaySound("jumpland", Actor, 1, rootActor.Position, hardLanding ? 1.0f : 0.6f/*, new Vector2(0.7f, 1.3f)*/);
		}

		private static void ApplyFriction(ref Vector3 velocity)
		{
			float currentSpeed = velocity.Length;

			float control = currentSpeed < stopspeed ? stopspeed : currentSpeed;
			var drop = control * friction * Time.DeltaTime;

			float newspeed = currentSpeed - drop;
			if (newspeed < 0)
				newspeed = 0;

			if (currentSpeed < 0.0001f)
				velocity *= 0;
			else
				velocity *= newspeed / currentSpeed;
		}

		private static void ApplyAirAcceleration(ref Vector3 velocity, Vector3 wishVelocity)
		{
			float wishspeed = wishVelocity.Length;
			if (wishspeed > maxAirSpeed)
				wishspeed = maxAirSpeed;

			Vector3 wishDir = wishVelocity.Normalized;
			float wishspeedAirControl = wishspeed;

			if (airAcceleration != 0)
			{
				// Q2+ air acceleration
				float accel = airAcceleration;
				if (Vector3.Dot(velocity, wishDir) < 0)
					accel = airStopAcceleration;

				if (airStrafeAcceleration != 0 && Mathf.Abs(wishVelocity.X) > 0 && wishVelocity.Y == 0)
				{
					// only strafe movement
					if (wishspeed > maxAirStrafeSpeed)
						wishspeed = maxAirStrafeSpeed;

					accel = airStrafeAcceleration;
				}

				ApplyAcceleration(ref velocity, wishDir, wishspeed, float.MaxValue, accel);
			}

			// QW air acceleration
			if (strafeAcceleration != 0f)
			{
				ApplyAcceleration(ref velocity, wishDir, wishspeed, maxAirStrafeSpeed,
					strafeAcceleration);
			}

			// air control while holding forward/back buttons
			//if (airControl != 0 && moveDirection.X == 0 && Mathf.Abs(moveDirection.Y) > 0)
			//	PM_Aircontrol(wishdir, wishspeedAirControl);

			// apply gravity
			velocity += Gravity * Time.DeltaTime;
			//Console.Print(Time.DeltaTime.ToString());
		}

		private static void ApplyAcceleration(ref Vector3 velocity, Vector3 wishDir, float wishspeed, float maxWishspeed,
			float acceleration)
		{
			float wishspeedOrig = wishspeed;
			if (wishspeed > maxWishspeed)
				wishspeed = maxWishspeed;

			float currentSpeed = Vector3.Dot(velocity, wishDir);
			float addSpeed = wishspeed - currentSpeed;
			if (addSpeed <= 0f)
				return;

			float accelSpeed = acceleration * wishspeedOrig * Time.DeltaTime;
			if (accelSpeed > addSpeed)
				accelSpeed = addSpeed;

			velocity += accelSpeed * wishDir;
		}
	}
}