Added shortest path calculation for NavGrid