void CalcMove(Vector3 waypoint, ref Vector3 refMovement, ref Quaternion refRotation) { Vector3 dir = waypoint - transform.position; dir.y = dir.y - PathHeight; dir.Normalize(); Vector3 vDirection = Vector3.Project(dir, transform.up); Vector3 lDirection = dir - vDirection; // Get rotation based on yaw angle. float yaw = MathHelper.AngleAroundAxis(transform.forward, lDirection, transform.up); refRotation = Quaternion.AngleAxis(yaw, transform.up); float moveSpeed = rootMotionMovement.magnitude / Time.deltaTime; if (moveSpeed == 0f) { moveSpeed = MovementSpeed; } // Set the final velocity based on the rotation we'll look at. Quaternion toRotation = transform.rotation * refRotation; refMovement = toRotation.Forward() * (moveSpeed * Time.deltaTime); }