Extracted the simple nav mesh system to it's own class.