Work on waypoint navigation and castle guard animation