getDistance() now applies correct cost for diagonal movement