Bugfix/AI navigation (#46)

* Make ClientMovementDriver.GetSpeed a public method
* Create new method MoveTowards in SimulatedPlayerDriver that sets/gets the agent.nextPosition to help constrain movement to the navmesh

Author: mattyoung-improbable

workers/unity/Assets/Fps/Scripts/SimulatedPlayer/SimulatedPlayerDriver.cs

@@ -84,6 +84,7 @@ private void OnRespawn(Empty empty)
         if (state == PlayerState.Dead)
         {
             anchorPoint = transform.position;
+            agent.Warp(transform.position);
             SetPlayerState(PlayerState.LookingForTarget);
         }
     }
@@ -140,13 +141,10 @@ private void Update_LookingForTarget()
     {
         if (!agent.isOnNavMesh)
         {
-            // If agent has fallen off the nav mesh, move forward and attempt to get back on the navmesh.
-            var velocity = transform.forward;
-            var rotation = Quaternion.LookRotation(velocity, Vector3.up);
-            var speed = sprintNext ? MovementSpeed.Sprint : MovementSpeed.Run;
-            movementDriver.ApplyMovement(velocity, rotation, speed, jumpNext);
-            jumpNext = false;
-            agent.Warp(transform.position);
+            if (NavMesh.SamplePosition(transform.position, out var hit, 0.5f, NavMesh.AllAreas))
+            {
+                agent.Warp(hit.position);
+            }
         }
         else if (agent.remainingDistance < MinRemainingDistance || agent.pathStatus == NavMeshPathStatus.PathInvalid ||
             !agent.hasPath)
@@ -161,7 +159,7 @@ private void Update_LookingForTarget()
             {
                 var rotation = Quaternion.LookRotation(velocity, Vector3.up);
                 var speed = sprintNext ? MovementSpeed.Sprint : MovementSpeed.Run;
-                movementDriver.ApplyMovement(velocity, rotation, speed, jumpNext);
+                MoveTowards(transform.position + velocity, speed, rotation, jumpNext);
                 jumpNext = false;
             }
         }
@@ -180,8 +178,9 @@ private void Update_ShootingTarget()
             var targetRotation = Quaternion.LookRotation(targetCenter - gunOrigin);
             var rotationAmount = Quaternion.RotateTowards(transform.rotation, targetRotation, 10f);
 
-            GetComponent<ClientMovementDriver>().ApplyMovement(
-                strafeRight ? transform.right : -transform.right, rotationAmount, MovementSpeed.Run, false);
+            var destination = transform.position + (strafeRight ? transform.right : -transform.right);
+
+            MoveTowards(destination, MovementSpeed.Run, rotationAmount, false);
 
             if (shooting.IsShooting(true) && Mathf.Abs(Quaternion.Angle(targetRotation, transform.rotation)) < 5)
             {
@@ -203,6 +202,21 @@ private void Update_ShootingTarget()
         }
     }
 
+    private void MoveTowards(Vector3 destination, MovementSpeed speed, Quaternion rotation, bool jump = false)
+    {
+        var agentPosition = agent.nextPosition;
+        var direction = (destination - agentPosition).normalized;
+        var desiredVelocity = direction * movementDriver.GetSpeed(speed);
+
+        // Setting nextPosition will move the agent towards destination, but is constrained by the navmesh
+        agent.nextPosition += desiredVelocity * Time.deltaTime;
+
+        // Getting nextPosition here will return the constrained position of the agent
+        var actualDirection = (agent.nextPosition - agentPosition).normalized;
+
+        movementDriver.ApplyMovement(actualDirection, rotation, speed, jump);
+    }
+
     private bool TargetIsValid()
     {
         if (target == null)
@@ -316,7 +330,9 @@ private IEnumerator CheckImmobility()
                 similarPositionsCount++;
                 if (similarPositionsCount >= maxSimilarPositions)
                 {
-                    Debug.LogWarningFormat("{0} got stuck at {1}, respawning", name, transform.position);
+                    Debug.LogWarningFormat("{0} got stuck at {1}: agent at {2}, respawning",
+                        name, transform.position, agent.nextPosition);
+
                     SetPlayerState(PlayerState.Dead);
                 }
             }

workers/unity/Packages/com.improbable.gdk.movement/Behaviours/ClientMovementDriver.cs

@@ -165,7 +165,7 @@ private void OnForcedRotation(RotationUpdate forcedRotation)
 
             cumulativeRotationTimeDelta = 0;
             pitchDirty = rollDirty = yawDirty = false;
-            
+
             //Apply the forced rotation
             var x = rotationConstraints.XAxisRotation ? forcedRotation.Pitch.ToFloat1k() : 0;
             var y = rotationConstraints.YAxisRotation ? forcedRotation.Yaw.ToFloat1k() : 0;
@@ -280,7 +280,7 @@ public void ProcessInput(Vector3 movement, Quaternion rotation, MovementSpeed mo
             CurrentRoll = rotation.eulerAngles.z;
         }
 
-        private float GetSpeed(MovementSpeed requestedSpeed)
+        public float GetSpeed(MovementSpeed requestedSpeed)
         {
             switch (requestedSpeed)
             {