refactoring. Also making sure that navNode won't be null before clampStep if you try to teleport to an invalid (according to the navmesh) location.