1 // Copyright 2010-2021 Google LLC
2 // Licensed under the Apache License, Version 2.0 (the "License");
3 // you may not use this file except in compliance with the License.
4 // You may obtain a copy of the License at
5 //
6 //     http://www.apache.org/licenses/LICENSE-2.0
7 //
8 // Unless required by applicable law or agreed to in writing, software
9 // distributed under the License is distributed on an "AS IS" BASIS,
10 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
11 // See the License for the specific language governing permissions and
12 // limitations under the License.
13 
14 // Implementation of all classes related to routing and search.
15 // This includes decision builders, local search neighborhood operators
16 // and local search filters.
17 // TODO(user): Move all existing routing search code here.
18 
19 #include "ortools/constraint_solver/routing_search.h"
20 
21 #include <algorithm>
22 #include <cmath>
23 #include <cstdint>
24 #include <deque>
25 #include <functional>
26 #include <iterator>
27 #include <limits>
28 #include <memory>
29 #include <numeric>
30 #include <queue>
31 #include <set>
32 #include <string>
33 #include <tuple>
34 #include <utility>
35 #include <vector>
36 
37 #include "absl/base/attributes.h"
38 #include "absl/container/flat_hash_map.h"
39 #include "absl/container/flat_hash_set.h"
40 #include "absl/flags/flag.h"
41 #include "absl/memory/memory.h"
42 #include "absl/meta/type_traits.h"
43 #include "absl/strings/str_cat.h"
44 #include "absl/strings/string_view.h"
45 #include "ortools/base/adjustable_priority_queue-inl.h"
46 #include "ortools/base/adjustable_priority_queue.h"
47 #include "ortools/base/integral_types.h"
48 #include "ortools/base/logging.h"
49 #include "ortools/base/macros.h"
50 #include "ortools/base/map_util.h"
51 #include "ortools/base/stl_util.h"
52 #include "ortools/constraint_solver/constraint_solver.h"
53 #include "ortools/constraint_solver/constraint_solveri.h"
54 #include "ortools/constraint_solver/routing.h"
55 #include "ortools/constraint_solver/routing_index_manager.h"
56 #include "ortools/constraint_solver/routing_types.h"
57 #include "ortools/graph/christofides.h"
58 #include "ortools/util/bitset.h"
59 #include "ortools/util/range_query_function.h"
60 #include "ortools/util/saturated_arithmetic.h"
61 
62 namespace operations_research {
63 class LocalSearchPhaseParameters;
64 }  // namespace operations_research
65 
66 ABSL_FLAG(bool, routing_shift_insertion_cost_by_penalty, true,
67           "Shift insertion costs by the penalty of the inserted node(s).");
68 
69 ABSL_FLAG(int64_t, sweep_sectors, 1,
70           "The number of sectors the space is divided into before it is sweeped"
71           " by the ray.");
72 
73 namespace operations_research {
74 
75 // --- VehicleTypeCurator ---
76 
Reset(const std::function<bool (int)> & store_vehicle)77 void VehicleTypeCurator::Reset(const std::function<bool(int)>& store_vehicle) {
78   const std::vector<std::set<VehicleClassEntry>>& all_vehicle_classes_per_type =
79       vehicle_type_container_->sorted_vehicle_classes_per_type;
80   sorted_vehicle_classes_per_type_.resize(all_vehicle_classes_per_type.size());
81   const std::vector<std::deque<int>>& all_vehicles_per_class =
82       vehicle_type_container_->vehicles_per_vehicle_class;
83   vehicles_per_vehicle_class_.resize(all_vehicles_per_class.size());
84 
85   for (int type = 0; type < all_vehicle_classes_per_type.size(); type++) {
86     std::set<VehicleClassEntry>& stored_class_entries =
87         sorted_vehicle_classes_per_type_[type];
88     stored_class_entries.clear();
89     for (VehicleClassEntry class_entry : all_vehicle_classes_per_type[type]) {
90       const int vehicle_class = class_entry.vehicle_class;
91       std::vector<int>& stored_vehicles =
92           vehicles_per_vehicle_class_[vehicle_class];
93       stored_vehicles.clear();
94       for (int vehicle : all_vehicles_per_class[vehicle_class]) {
95         if (store_vehicle(vehicle)) {
96           stored_vehicles.push_back(vehicle);
97         }
98       }
99       if (!stored_vehicles.empty()) {
100         stored_class_entries.insert(class_entry);
101       }
102     }
103   }
104 }
105 
Update(const std::function<bool (int)> & remove_vehicle)106 void VehicleTypeCurator::Update(
107     const std::function<bool(int)>& remove_vehicle) {
108   for (std::set<VehicleClassEntry>& class_entries :
109        sorted_vehicle_classes_per_type_) {
110     auto class_entry_it = class_entries.begin();
111     while (class_entry_it != class_entries.end()) {
112       const int vehicle_class = class_entry_it->vehicle_class;
113       std::vector<int>& vehicles = vehicles_per_vehicle_class_[vehicle_class];
114       vehicles.erase(std::remove_if(vehicles.begin(), vehicles.end(),
115                                     [&remove_vehicle](int vehicle) {
116                                       return remove_vehicle(vehicle);
117                                     }),
118                      vehicles.end());
119       if (vehicles.empty()) {
120         class_entry_it = class_entries.erase(class_entry_it);
121       } else {
122         class_entry_it++;
123       }
124     }
125   }
126 }
127 
HasCompatibleVehicleOfType(int type,const std::function<bool (int)> & vehicle_is_compatible) const128 bool VehicleTypeCurator::HasCompatibleVehicleOfType(
129     int type, const std::function<bool(int)>& vehicle_is_compatible) const {
130   for (const VehicleClassEntry& vehicle_class_entry :
131        sorted_vehicle_classes_per_type_[type]) {
132     for (int vehicle :
133          vehicles_per_vehicle_class_[vehicle_class_entry.vehicle_class]) {
134       if (vehicle_is_compatible(vehicle)) return true;
135     }
136   }
137   return false;
138 }
139 
GetCompatibleVehicleOfType(int type,std::function<bool (int)> vehicle_is_compatible,std::function<bool (int)> stop_and_return_vehicle)140 std::pair<int, int> VehicleTypeCurator::GetCompatibleVehicleOfType(
141     int type, std::function<bool(int)> vehicle_is_compatible,
142     std::function<bool(int)> stop_and_return_vehicle) {
143   std::set<VehicleTypeCurator::VehicleClassEntry>& sorted_classes =
144       sorted_vehicle_classes_per_type_[type];
145   auto vehicle_class_it = sorted_classes.begin();
146 
147   while (vehicle_class_it != sorted_classes.end()) {
148     const int vehicle_class = vehicle_class_it->vehicle_class;
149     std::vector<int>& vehicles = vehicles_per_vehicle_class_[vehicle_class];
150     DCHECK(!vehicles.empty());
151 
152     for (auto vehicle_it = vehicles.begin(); vehicle_it != vehicles.end();
153          vehicle_it++) {
154       const int vehicle = *vehicle_it;
155       if (vehicle_is_compatible(vehicle)) {
156         vehicles.erase(vehicle_it);
157         if (vehicles.empty()) {
158           sorted_classes.erase(vehicle_class_it);
159         }
160         return {vehicle, -1};
161       }
162       if (stop_and_return_vehicle(vehicle)) {
163         return {-1, vehicle};
164       }
165     }
166     // If no compatible vehicle was found in this class, move on to the next
167     // vehicle class.
168     vehicle_class_it++;
169   }
170   // No compatible vehicle of the given type was found and the stopping
171   // condition wasn't met.
172   return {-1, -1};
173 }
174 
175 // - Models with pickup/deliveries or node precedences are best handled by
176 //   PARALLEL_CHEAPEST_INSERTION.
177 // - As of January 2018, models with single nodes and at least one node with
178 //   only one allowed vehicle are better solved by PATH_MOST_CONSTRAINED_ARC.
179 // - In all other cases, PATH_CHEAPEST_ARC is used.
180 // TODO(user): Make this smarter.
AutomaticFirstSolutionStrategy(bool has_pickup_deliveries,bool has_node_precedences,bool has_single_vehicle_node)181 FirstSolutionStrategy::Value AutomaticFirstSolutionStrategy(
182     bool has_pickup_deliveries, bool has_node_precedences,
183     bool has_single_vehicle_node) {
184   if (has_pickup_deliveries || has_node_precedences) {
185     return FirstSolutionStrategy::PARALLEL_CHEAPEST_INSERTION;
186   }
187   if (has_single_vehicle_node) {
188     return FirstSolutionStrategy::PATH_MOST_CONSTRAINED_ARC;
189   }
190   return FirstSolutionStrategy::PATH_CHEAPEST_ARC;
191 }
192 
193 // --- First solution decision builder ---
194 
195 // IntVarFilteredDecisionBuilder
196 
IntVarFilteredDecisionBuilder(std::unique_ptr<IntVarFilteredHeuristic> heuristic)197 IntVarFilteredDecisionBuilder::IntVarFilteredDecisionBuilder(
198     std::unique_ptr<IntVarFilteredHeuristic> heuristic)
199     : heuristic_(std::move(heuristic)) {}
200 
Next(Solver * solver)201 Decision* IntVarFilteredDecisionBuilder::Next(Solver* solver) {
202   Assignment* const assignment = heuristic_->BuildSolution();
203   if (assignment != nullptr) {
204     VLOG(2) << "Number of decisions: " << heuristic_->number_of_decisions();
205     VLOG(2) << "Number of rejected decisions: "
206             << heuristic_->number_of_rejects();
207     assignment->Restore();
208   } else {
209     solver->Fail();
210   }
211   return nullptr;
212 }
213 
number_of_decisions() const214 int64_t IntVarFilteredDecisionBuilder::number_of_decisions() const {
215   return heuristic_->number_of_decisions();
216 }
217 
number_of_rejects() const218 int64_t IntVarFilteredDecisionBuilder::number_of_rejects() const {
219   return heuristic_->number_of_rejects();
220 }
221 
DebugString() const222 std::string IntVarFilteredDecisionBuilder::DebugString() const {
223   return absl::StrCat("IntVarFilteredDecisionBuilder(",
224                       heuristic_->DebugString(), ")");
225 }
226 
227 // --- First solution heuristics ---
228 
229 // IntVarFilteredHeuristic
230 
IntVarFilteredHeuristic(Solver * solver,const std::vector<IntVar * > & vars,LocalSearchFilterManager * filter_manager)231 IntVarFilteredHeuristic::IntVarFilteredHeuristic(
232     Solver* solver, const std::vector<IntVar*>& vars,
233     LocalSearchFilterManager* filter_manager)
234     : assignment_(solver->MakeAssignment()),
235       solver_(solver),
236       vars_(vars),
237       delta_(solver->MakeAssignment()),
238       is_in_delta_(vars_.size(), false),
239       empty_(solver->MakeAssignment()),
240       filter_manager_(filter_manager),
241       number_of_decisions_(0),
242       number_of_rejects_(0) {
243   assignment_->MutableIntVarContainer()->Resize(vars_.size());
244   delta_indices_.reserve(vars_.size());
245 }
246 
ResetSolution()247 void IntVarFilteredHeuristic::ResetSolution() {
248   number_of_decisions_ = 0;
249   number_of_rejects_ = 0;
250   // Wiping assignment when starting a new search.
251   assignment_->MutableIntVarContainer()->Clear();
252   assignment_->MutableIntVarContainer()->Resize(vars_.size());
253   delta_->MutableIntVarContainer()->Clear();
254   SynchronizeFilters();
255 }
256 
BuildSolution()257 Assignment* const IntVarFilteredHeuristic::BuildSolution() {
258   ResetSolution();
259   if (!InitializeSolution()) {
260     return nullptr;
261   }
262   SynchronizeFilters();
263   if (BuildSolutionInternal()) {
264     return assignment_;
265   }
266   return nullptr;
267 }
268 
BuildSolutionFromRoutes(const std::function<int64_t (int64_t)> & next_accessor)269 const Assignment* RoutingFilteredHeuristic::BuildSolutionFromRoutes(
270     const std::function<int64_t(int64_t)>& next_accessor) {
271   ResetSolution();
272   ResetVehicleIndices();
273   // NOTE: We don't need to clear or pre-set the two following vectors as the
274   // for loop below will set all elements.
275   start_chain_ends_.resize(model()->vehicles());
276   end_chain_starts_.resize(model()->vehicles());
277 
278   for (int v = 0; v < model_->vehicles(); v++) {
279     int64_t node = model_->Start(v);
280     while (!model_->IsEnd(node)) {
281       const int64_t next = next_accessor(node);
282       DCHECK_NE(next, node);
283       SetValue(node, next);
284       SetVehicleIndex(node, v);
285       node = next;
286     }
287     // We relax all routes from start to end, so routes can now be extended
288     // by inserting nodes between the start and end.
289     start_chain_ends_[v] = model()->Start(v);
290     end_chain_starts_[v] = model()->End(v);
291   }
292   if (!Commit()) {
293     return nullptr;
294   }
295   SynchronizeFilters();
296   if (BuildSolutionInternal()) {
297     return assignment_;
298   }
299   return nullptr;
300 }
301 
Commit()302 bool IntVarFilteredHeuristic::Commit() {
303   ++number_of_decisions_;
304   const bool accept = FilterAccept();
305   if (accept) {
306     const Assignment::IntContainer& delta_container = delta_->IntVarContainer();
307     const int delta_size = delta_container.Size();
308     Assignment::IntContainer* const container =
309         assignment_->MutableIntVarContainer();
310     for (int i = 0; i < delta_size; ++i) {
311       const IntVarElement& delta_element = delta_container.Element(i);
312       IntVar* const var = delta_element.Var();
313       DCHECK_EQ(var, vars_[delta_indices_[i]]);
314       container->AddAtPosition(var, delta_indices_[i])
315           ->SetValue(delta_element.Value());
316     }
317     SynchronizeFilters();
318   } else {
319     ++number_of_rejects_;
320   }
321   // Reset is_in_delta to all false.
322   for (const int delta_index : delta_indices_) {
323     is_in_delta_[delta_index] = false;
324   }
325   delta_->Clear();
326   delta_indices_.clear();
327   return accept;
328 }
329 
SynchronizeFilters()330 void IntVarFilteredHeuristic::SynchronizeFilters() {
331   if (filter_manager_) filter_manager_->Synchronize(assignment_, delta_);
332 }
333 
FilterAccept()334 bool IntVarFilteredHeuristic::FilterAccept() {
335   if (!filter_manager_) return true;
336   LocalSearchMonitor* const monitor = solver_->GetLocalSearchMonitor();
337   return filter_manager_->Accept(monitor, delta_, empty_,
338                                  std::numeric_limits<int64_t>::min(),
339                                  std::numeric_limits<int64_t>::max());
340 }
341 
342 // RoutingFilteredHeuristic
343 
RoutingFilteredHeuristic(RoutingModel * model,LocalSearchFilterManager * filter_manager)344 RoutingFilteredHeuristic::RoutingFilteredHeuristic(
345     RoutingModel* model, LocalSearchFilterManager* filter_manager)
346     : IntVarFilteredHeuristic(model->solver(), model->Nexts(), filter_manager),
347       model_(model) {}
348 
InitializeSolution()349 bool RoutingFilteredHeuristic::InitializeSolution() {
350   // Find the chains of nodes (when nodes have their "Next" value bound in the
351   // current solution, it forms a link in a chain). Eventually, starts[end]
352   // will contain the index of the first node of the chain ending at node 'end'
353   // and ends[start] will be the last node of the chain starting at node
354   // 'start'. Values of starts[node] and ends[node] for other nodes is used
355   // for intermediary computations and do not necessarily reflect actual chain
356   // starts and ends.
357 
358   // Start by adding partial start chains to current assignment.
359   start_chain_ends_.clear();
360   start_chain_ends_.resize(model()->vehicles(), -1);
361   end_chain_starts_.clear();
362   end_chain_starts_.resize(model()->vehicles(), -1);
363 
364   ResetVehicleIndices();
365   for (int vehicle = 0; vehicle < model()->vehicles(); ++vehicle) {
366     int64_t node = model()->Start(vehicle);
367     while (!model()->IsEnd(node) && Var(node)->Bound()) {
368       const int64_t next = Var(node)->Min();
369       SetValue(node, next);
370       SetVehicleIndex(node, vehicle);
371       node = next;
372     }
373     start_chain_ends_[vehicle] = node;
374   }
375 
376   std::vector<int64_t> starts(Size() + model()->vehicles(), -1);
377   std::vector<int64_t> ends(Size() + model()->vehicles(), -1);
378   for (int node = 0; node < Size() + model()->vehicles(); ++node) {
379     // Each node starts as a singleton chain.
380     starts[node] = node;
381     ends[node] = node;
382   }
383   std::vector<bool> touched(Size(), false);
384   for (int node = 0; node < Size(); ++node) {
385     int current = node;
386     while (!model()->IsEnd(current) && !touched[current]) {
387       touched[current] = true;
388       IntVar* const next_var = Var(current);
389       if (next_var->Bound()) {
390         current = next_var->Value();
391       }
392     }
393     // Merge the sub-chain starting from 'node' and ending at 'current' with
394     // the existing sub-chain starting at 'current'.
395     starts[ends[current]] = starts[node];
396     ends[starts[node]] = ends[current];
397   }
398 
399   // Set each route to be the concatenation of the chain at its starts and the
400   // chain at its end, without nodes in between.
401   for (int vehicle = 0; vehicle < model()->vehicles(); ++vehicle) {
402     end_chain_starts_[vehicle] = starts[model()->End(vehicle)];
403     int64_t node = start_chain_ends_[vehicle];
404     if (!model()->IsEnd(node)) {
405       int64_t next = starts[model()->End(vehicle)];
406       SetValue(node, next);
407       SetVehicleIndex(node, vehicle);
408       node = next;
409       while (!model()->IsEnd(node)) {
410         next = Var(node)->Min();
411         SetValue(node, next);
412         SetVehicleIndex(node, vehicle);
413         node = next;
414       }
415     }
416   }
417 
418   if (!Commit()) {
419     ResetVehicleIndices();
420     return false;
421   }
422   return true;
423 }
424 
MakeDisjunctionNodesUnperformed(int64_t node)425 void RoutingFilteredHeuristic::MakeDisjunctionNodesUnperformed(int64_t node) {
426   model()->ForEachNodeInDisjunctionWithMaxCardinalityFromIndex(
427       node, 1, [this, node](int alternate) {
428         if (node != alternate && !Contains(alternate)) {
429           SetValue(alternate, alternate);
430         }
431       });
432 }
433 
MakeUnassignedNodesUnperformed()434 void RoutingFilteredHeuristic::MakeUnassignedNodesUnperformed() {
435   for (int index = 0; index < Size(); ++index) {
436     if (!Contains(index)) {
437       SetValue(index, index);
438     }
439   }
440 }
441 
MakePartiallyPerformedPairsUnperformed()442 void RoutingFilteredHeuristic::MakePartiallyPerformedPairsUnperformed() {
443   std::vector<bool> to_make_unperformed(Size(), false);
444   for (const auto& [pickups, deliveries] :
445        model()->GetPickupAndDeliveryPairs()) {
446     int64_t performed_pickup = -1;
447     for (int64_t pickup : pickups) {
448       if (Contains(pickup) && Value(pickup) != pickup) {
449         performed_pickup = pickup;
450         break;
451       }
452     }
453     int64_t performed_delivery = -1;
454     for (int64_t delivery : deliveries) {
455       if (Contains(delivery) && Value(delivery) != delivery) {
456         performed_delivery = delivery;
457         break;
458       }
459     }
460     if ((performed_pickup == -1) != (performed_delivery == -1)) {
461       if (performed_pickup != -1) {
462         to_make_unperformed[performed_pickup] = true;
463       }
464       if (performed_delivery != -1) {
465         to_make_unperformed[performed_delivery] = true;
466       }
467     }
468   }
469   for (int index = 0; index < Size(); ++index) {
470     if (to_make_unperformed[index] || !Contains(index)) continue;
471     int64_t next = Value(index);
472     while (next < Size() && to_make_unperformed[next]) {
473       const int64_t next_of_next = Value(next);
474       SetValue(index, next_of_next);
475       SetValue(next, next);
476       next = next_of_next;
477     }
478   }
479 }
480 
481 // CheapestInsertionFilteredHeuristic
482 
CheapestInsertionFilteredHeuristic(RoutingModel * model,std::function<int64_t (int64_t,int64_t,int64_t)> evaluator,std::function<int64_t (int64_t)> penalty_evaluator,LocalSearchFilterManager * filter_manager)483 CheapestInsertionFilteredHeuristic::CheapestInsertionFilteredHeuristic(
484     RoutingModel* model,
485     std::function<int64_t(int64_t, int64_t, int64_t)> evaluator,
486     std::function<int64_t(int64_t)> penalty_evaluator,
487     LocalSearchFilterManager* filter_manager)
488     : RoutingFilteredHeuristic(model, filter_manager),
489       evaluator_(std::move(evaluator)),
490       penalty_evaluator_(std::move(penalty_evaluator)) {}
491 
492 std::vector<std::vector<CheapestInsertionFilteredHeuristic::StartEndValue>>
ComputeStartEndDistanceForVehicles(const std::vector<int> & vehicles)493 CheapestInsertionFilteredHeuristic::ComputeStartEndDistanceForVehicles(
494     const std::vector<int>& vehicles) {
495   const absl::flat_hash_set<int> vehicle_set(vehicles.begin(), vehicles.end());
496   std::vector<std::vector<StartEndValue>> start_end_distances_per_node(
497       model()->Size());
498 
499   for (int node = 0; node < model()->Size(); node++) {
500     if (Contains(node)) continue;
501     std::vector<StartEndValue>& start_end_distances =
502         start_end_distances_per_node[node];
503     const IntVar* const vehicle_var = model()->VehicleVar(node);
504     const int64_t num_allowed_vehicles = vehicle_var->Size();
505 
506     const auto add_distance = [this, node, num_allowed_vehicles,
507                                &start_end_distances](int vehicle) {
508       const int64_t start = model()->Start(vehicle);
509       const int64_t end = model()->End(vehicle);
510 
511       // We compute the distance of node to the start/end nodes of the route.
512       const int64_t distance =
513           CapAdd(model()->GetArcCostForVehicle(start, node, vehicle),
514                  model()->GetArcCostForVehicle(node, end, vehicle));
515       start_end_distances.push_back({num_allowed_vehicles, distance, vehicle});
516     };
517     // Iterating over an IntVar domain is faster than calling Contains.
518     // Therefore we iterate on 'vehicles' only if it's smaller than the domain
519     // size of the VehicleVar.
520     if (num_allowed_vehicles < vehicles.size()) {
521       std::unique_ptr<IntVarIterator> it(
522           vehicle_var->MakeDomainIterator(false));
523       for (const int64_t vehicle : InitAndGetValues(it.get())) {
524         if (vehicle < 0 || !vehicle_set.contains(vehicle)) continue;
525         add_distance(vehicle);
526       }
527     } else {
528       for (const int vehicle : vehicles) {
529         if (!vehicle_var->Contains(vehicle)) continue;
530         add_distance(vehicle);
531       }
532     }
533     // Sort the distances for the node to all start/ends of available vehicles
534     // in decreasing order.
535     std::sort(start_end_distances.begin(), start_end_distances.end(),
536               [](const StartEndValue& first, const StartEndValue& second) {
537                 return second < first;
538               });
539   }
540   return start_end_distances_per_node;
541 }
542 
543 template <class Queue>
InitializePriorityQueue(std::vector<std::vector<StartEndValue>> * start_end_distances_per_node,Queue * priority_queue)544 void CheapestInsertionFilteredHeuristic::InitializePriorityQueue(
545     std::vector<std::vector<StartEndValue>>* start_end_distances_per_node,
546     Queue* priority_queue) {
547   const int num_nodes = model()->Size();
548   DCHECK_EQ(start_end_distances_per_node->size(), num_nodes);
549 
550   for (int node = 0; node < num_nodes; node++) {
551     if (Contains(node)) continue;
552     std::vector<StartEndValue>& start_end_distances =
553         (*start_end_distances_per_node)[node];
554     if (start_end_distances.empty()) {
555       continue;
556     }
557     // Put the best StartEndValue for this node in the priority queue.
558     const StartEndValue& start_end_value = start_end_distances.back();
559     priority_queue->push(std::make_pair(start_end_value, node));
560     start_end_distances.pop_back();
561   }
562 }
563 
InsertBetween(int64_t node,int64_t predecessor,int64_t successor)564 void CheapestInsertionFilteredHeuristic::InsertBetween(int64_t node,
565                                                        int64_t predecessor,
566                                                        int64_t successor) {
567   SetValue(predecessor, node);
568   SetValue(node, successor);
569   MakeDisjunctionNodesUnperformed(node);
570 }
571 
AppendInsertionPositionsAfter(int64_t node_to_insert,int64_t start,int64_t next_after_start,int vehicle,std::vector<NodeInsertion> * node_insertions)572 void CheapestInsertionFilteredHeuristic::AppendInsertionPositionsAfter(
573     int64_t node_to_insert, int64_t start, int64_t next_after_start,
574     int vehicle, std::vector<NodeInsertion>* node_insertions) {
575   DCHECK(node_insertions != nullptr);
576   int64_t insert_after = start;
577   while (!model()->IsEnd(insert_after)) {
578     const int64_t insert_before =
579         (insert_after == start) ? next_after_start : Value(insert_after);
580     node_insertions->push_back(
581         {insert_after, vehicle,
582          GetInsertionCostForNodeAtPosition(node_to_insert, insert_after,
583                                            insert_before, vehicle)});
584     insert_after = insert_before;
585   }
586 }
587 
GetInsertionCostForNodeAtPosition(int64_t node_to_insert,int64_t insert_after,int64_t insert_before,int vehicle) const588 int64_t CheapestInsertionFilteredHeuristic::GetInsertionCostForNodeAtPosition(
589     int64_t node_to_insert, int64_t insert_after, int64_t insert_before,
590     int vehicle) const {
591   return CapSub(CapAdd(evaluator_(insert_after, node_to_insert, vehicle),
592                        evaluator_(node_to_insert, insert_before, vehicle)),
593                 evaluator_(insert_after, insert_before, vehicle));
594 }
595 
GetUnperformedValue(int64_t node_to_insert) const596 int64_t CheapestInsertionFilteredHeuristic::GetUnperformedValue(
597     int64_t node_to_insert) const {
598   if (penalty_evaluator_ != nullptr) {
599     return penalty_evaluator_(node_to_insert);
600   }
601   return std::numeric_limits<int64_t>::max();
602 }
603 
604 namespace {
605 template <class T>
SortAndExtractPairSeconds(std::vector<std::pair<int64_t,T>> * pairs,std::vector<T> * sorted_seconds)606 void SortAndExtractPairSeconds(std::vector<std::pair<int64_t, T>>* pairs,
607                                std::vector<T>* sorted_seconds) {
608   CHECK(pairs != nullptr);
609   CHECK(sorted_seconds != nullptr);
610   std::sort(pairs->begin(), pairs->end());
611   sorted_seconds->reserve(pairs->size());
612   for (const std::pair<int64_t, T>& p : *pairs) {
613     sorted_seconds->push_back(p.second);
614   }
615 }
616 }  // namespace
617 
618 // Priority queue entries used by global cheapest insertion heuristic.
619 
620 // Entry in priority queue containing the insertion positions of a node pair.
621 class GlobalCheapestInsertionFilteredHeuristic::PairEntry {
622  public:
PairEntry(int pickup_to_insert,int pickup_insert_after,int delivery_to_insert,int delivery_insert_after,int vehicle,int64_t bucket)623   PairEntry(int pickup_to_insert, int pickup_insert_after,
624             int delivery_to_insert, int delivery_insert_after, int vehicle,
625             int64_t bucket)
626       : value_(std::numeric_limits<int64_t>::max()),
627         heap_index_(-1),
628         pickup_to_insert_(pickup_to_insert),
629         pickup_insert_after_(pickup_insert_after),
630         delivery_to_insert_(delivery_to_insert),
631         delivery_insert_after_(delivery_insert_after),
632         vehicle_(vehicle),
633         bucket_(bucket) {}
634   // Note: for compatibility reasons, comparator follows tie-breaking rules used
635   // in the first version of GlobalCheapestInsertion.
operator <(const PairEntry & other) const636   bool operator<(const PairEntry& other) const {
637     // We give higher priority to insertions from lower buckets.
638     if (bucket_ != other.bucket_) {
639       return bucket_ > other.bucket_;
640     }
641     // We then compare by value, then we favor insertions (vehicle != -1).
642     // The rest of the tie-breaking is done with std::tie.
643     if (value_ != other.value_) {
644       return value_ > other.value_;
645     }
646     if ((vehicle_ == -1) ^ (other.vehicle_ == -1)) {
647       return vehicle_ == -1;
648     }
649     return std::tie(pickup_insert_after_, pickup_to_insert_,
650                     delivery_insert_after_, delivery_to_insert_, vehicle_) >
651            std::tie(other.pickup_insert_after_, other.pickup_to_insert_,
652                     other.delivery_insert_after_, other.delivery_to_insert_,
653                     other.vehicle_);
654   }
SetHeapIndex(int h)655   void SetHeapIndex(int h) { heap_index_ = h; }
GetHeapIndex() const656   int GetHeapIndex() const { return heap_index_; }
set_value(int64_t value)657   void set_value(int64_t value) { value_ = value; }
pickup_to_insert() const658   int pickup_to_insert() const { return pickup_to_insert_; }
pickup_insert_after() const659   int pickup_insert_after() const { return pickup_insert_after_; }
set_pickup_insert_after(int pickup_insert_after)660   void set_pickup_insert_after(int pickup_insert_after) {
661     pickup_insert_after_ = pickup_insert_after;
662   }
delivery_to_insert() const663   int delivery_to_insert() const { return delivery_to_insert_; }
delivery_insert_after() const664   int delivery_insert_after() const { return delivery_insert_after_; }
vehicle() const665   int vehicle() const { return vehicle_; }
set_vehicle(int vehicle)666   void set_vehicle(int vehicle) { vehicle_ = vehicle; }
667 
668  private:
669   int64_t value_;
670   int heap_index_;
671   const int pickup_to_insert_;
672   int pickup_insert_after_;
673   const int delivery_to_insert_;
674   const int delivery_insert_after_;
675   int vehicle_;
676   const int64_t bucket_;
677 };
678 
679 // Entry in priority queue containing the insertion position of a node.
680 class GlobalCheapestInsertionFilteredHeuristic::NodeEntry {
681  public:
NodeEntry(int node_to_insert,int insert_after,int vehicle,int64_t bucket)682   NodeEntry(int node_to_insert, int insert_after, int vehicle, int64_t bucket)
683       : value_(std::numeric_limits<int64_t>::max()),
684         heap_index_(-1),
685         node_to_insert_(node_to_insert),
686         insert_after_(insert_after),
687         vehicle_(vehicle),
688         bucket_(bucket) {}
operator <(const NodeEntry & other) const689   bool operator<(const NodeEntry& other) const {
690     // See PairEntry::operator<(), above. This one is similar.
691     if (bucket_ != other.bucket_) {
692       return bucket_ > other.bucket_;
693     }
694     if (value_ != other.value_) {
695       return value_ > other.value_;
696     }
697     if ((vehicle_ == -1) ^ (other.vehicle_ == -1)) {
698       return vehicle_ == -1;
699     }
700     return std::tie(insert_after_, node_to_insert_, vehicle_) >
701            std::tie(other.insert_after_, other.node_to_insert_, other.vehicle_);
702   }
SetHeapIndex(int h)703   void SetHeapIndex(int h) { heap_index_ = h; }
GetHeapIndex() const704   int GetHeapIndex() const { return heap_index_; }
set_value(int64_t value)705   void set_value(int64_t value) { value_ = value; }
node_to_insert() const706   int node_to_insert() const { return node_to_insert_; }
insert_after() const707   int insert_after() const { return insert_after_; }
set_insert_after(int insert_after)708   void set_insert_after(int insert_after) { insert_after_ = insert_after; }
vehicle() const709   int vehicle() const { return vehicle_; }
set_vehicle(int vehicle)710   void set_vehicle(int vehicle) { vehicle_ = vehicle; }
711 
712  private:
713   int64_t value_;
714   int heap_index_;
715   const int node_to_insert_;
716   int insert_after_;
717   int vehicle_;
718   const int64_t bucket_;
719 };
720 
721 // GlobalCheapestInsertionFilteredHeuristic
722 
723 GlobalCheapestInsertionFilteredHeuristic::
GlobalCheapestInsertionFilteredHeuristic(RoutingModel * model,std::function<int64_t (int64_t,int64_t,int64_t)> evaluator,std::function<int64_t (int64_t)> penalty_evaluator,LocalSearchFilterManager * filter_manager,GlobalCheapestInsertionParameters parameters)724     GlobalCheapestInsertionFilteredHeuristic(
725         RoutingModel* model,
726         std::function<int64_t(int64_t, int64_t, int64_t)> evaluator,
727         std::function<int64_t(int64_t)> penalty_evaluator,
728         LocalSearchFilterManager* filter_manager,
729         GlobalCheapestInsertionParameters parameters)
730     : CheapestInsertionFilteredHeuristic(model, std::move(evaluator),
731                                          std::move(penalty_evaluator),
732                                          filter_manager),
733       gci_params_(parameters),
734       node_index_to_vehicle_(model->Size(), -1),
735       empty_vehicle_type_curator_(nullptr) {
736   CHECK_GT(gci_params_.neighbors_ratio, 0);
737   CHECK_LE(gci_params_.neighbors_ratio, 1);
738   CHECK_GE(gci_params_.min_neighbors, 1);
739 
740   if (NumNeighbors() >= NumNonStartEndNodes() - 1) {
741     // All nodes are neighbors, so we set the neighbors_ratio to 1 to avoid
742     // unnecessary computations in the code.
743     gci_params_.neighbors_ratio = 1;
744   }
745 
746   if (gci_params_.neighbors_ratio == 1) {
747     gci_params_.use_neighbors_ratio_for_initialization = false;
748     all_nodes_.resize(model->Size());
749     std::iota(all_nodes_.begin(), all_nodes_.end(), 0);
750   }
751 }
752 
ComputeNeighborhoods()753 void GlobalCheapestInsertionFilteredHeuristic::ComputeNeighborhoods() {
754   if (gci_params_.neighbors_ratio == 1 ||
755       !node_index_to_neighbors_by_cost_class_.empty()) {
756     // Neighborhood computations not needed or already done.
757     return;
758   }
759 
760   // TODO(user): Refactor the neighborhood computations in RoutingModel.
761   const int64_t num_neighbors = NumNeighbors();
762   // If num_neighbors was greater or equal num_non_start_end_nodes - 1,
763   // gci_params_.neighbors_ratio should have been set to 1.
764   DCHECK_LT(num_neighbors, NumNonStartEndNodes() - 1);
765 
766   const RoutingModel& routing_model = *model();
767   const int64_t size = routing_model.Size();
768   node_index_to_neighbors_by_cost_class_.resize(size);
769   const int num_cost_classes = routing_model.GetCostClassesCount();
770   for (int64_t node_index = 0; node_index < size; node_index++) {
771     node_index_to_neighbors_by_cost_class_[node_index].resize(num_cost_classes);
772     for (int cc = 0; cc < num_cost_classes; cc++) {
773       node_index_to_neighbors_by_cost_class_[node_index][cc] =
774           absl::make_unique<SparseBitset<int64_t>>(size);
775     }
776   }
777 
778   for (int64_t node_index = 0; node_index < size; ++node_index) {
779     DCHECK(!routing_model.IsEnd(node_index));
780     if (routing_model.IsStart(node_index)) {
781       // We don't compute neighbors for vehicle starts: all nodes are considered
782       // neighbors for a vehicle start.
783       continue;
784     }
785 
786     // TODO(user): Use the model's IndexNeighborFinder when available.
787     for (int cost_class = 0; cost_class < num_cost_classes; cost_class++) {
788       if (!routing_model.HasVehicleWithCostClassIndex(
789               RoutingCostClassIndex(cost_class))) {
790         // No vehicle with this cost class, avoid unnecessary computations.
791         continue;
792       }
793       std::vector<std::pair</*cost*/ int64_t, /*node*/ int64_t>>
794           costed_after_nodes;
795       costed_after_nodes.reserve(size);
796       for (int after_node = 0; after_node < size; ++after_node) {
797         if (after_node != node_index && !routing_model.IsStart(after_node)) {
798           costed_after_nodes.push_back(
799               std::make_pair(routing_model.GetArcCostForClass(
800                                  node_index, after_node, cost_class),
801                              after_node));
802         }
803       }
804       std::nth_element(costed_after_nodes.begin(),
805                        costed_after_nodes.begin() + num_neighbors - 1,
806                        costed_after_nodes.end());
807       costed_after_nodes.resize(num_neighbors);
808 
809       for (auto [cost, neighbor] : costed_after_nodes) {
810         node_index_to_neighbors_by_cost_class_[node_index][cost_class]->Set(
811             neighbor);
812 
813         // Add reverse neighborhood.
814         DCHECK(!routing_model.IsEnd(neighbor) &&
815                !routing_model.IsStart(neighbor));
816         node_index_to_neighbors_by_cost_class_[neighbor][cost_class]->Set(
817             node_index);
818       }
819       // Add all vehicle starts as neighbors to this node and vice-versa.
820       for (int vehicle = 0; vehicle < routing_model.vehicles(); vehicle++) {
821         const int64_t vehicle_start = routing_model.Start(vehicle);
822         node_index_to_neighbors_by_cost_class_[node_index][cost_class]->Set(
823             vehicle_start);
824         node_index_to_neighbors_by_cost_class_[vehicle_start][cost_class]->Set(
825             node_index);
826       }
827     }
828   }
829 }
830 
IsNeighborForCostClass(int cost_class,int64_t node_index,int64_t neighbor_index) const831 bool GlobalCheapestInsertionFilteredHeuristic::IsNeighborForCostClass(
832     int cost_class, int64_t node_index, int64_t neighbor_index) const {
833   return gci_params_.neighbors_ratio == 1 ||
834          (*node_index_to_neighbors_by_cost_class_[node_index]
835                                                  [cost_class])[neighbor_index];
836 }
837 
CheckVehicleIndices() const838 bool GlobalCheapestInsertionFilteredHeuristic::CheckVehicleIndices() const {
839   std::vector<bool> node_is_visited(model()->Size(), -1);
840   for (int v = 0; v < model()->vehicles(); v++) {
841     for (int node = model()->Start(v); !model()->IsEnd(node);
842          node = Value(node)) {
843       if (node_index_to_vehicle_[node] != v) {
844         return false;
845       }
846       node_is_visited[node] = true;
847     }
848   }
849 
850   for (int node = 0; node < model()->Size(); node++) {
851     if (!node_is_visited[node] && node_index_to_vehicle_[node] != -1) {
852       return false;
853     }
854   }
855 
856   return true;
857 }
858 
BuildSolutionInternal()859 bool GlobalCheapestInsertionFilteredHeuristic::BuildSolutionInternal() {
860   ComputeNeighborhoods();
861   if (empty_vehicle_type_curator_ == nullptr) {
862     empty_vehicle_type_curator_ = absl::make_unique<VehicleTypeCurator>(
863         model()->GetVehicleTypeContainer());
864   }
865   // Store all empty vehicles in the empty_vehicle_type_curator_.
866   empty_vehicle_type_curator_->Reset(
867       [this](int vehicle) { return VehicleIsEmpty(vehicle); });
868   // Insert partially inserted pairs.
869   const RoutingModel::IndexPairs& pickup_delivery_pairs =
870       model()->GetPickupAndDeliveryPairs();
871   std::map<int64_t, std::vector<int>> pairs_to_insert_by_bucket;
872   absl::flat_hash_map<int, std::map<int64_t, std::vector<int>>>
873       vehicle_to_pair_nodes;
874   for (int index = 0; index < pickup_delivery_pairs.size(); index++) {
875     const RoutingModel::IndexPair& index_pair = pickup_delivery_pairs[index];
876     int pickup_vehicle = -1;
877     for (int64_t pickup : index_pair.first) {
878       if (Contains(pickup)) {
879         pickup_vehicle = node_index_to_vehicle_[pickup];
880         break;
881       }
882     }
883     int delivery_vehicle = -1;
884     for (int64_t delivery : index_pair.second) {
885       if (Contains(delivery)) {
886         delivery_vehicle = node_index_to_vehicle_[delivery];
887         break;
888       }
889     }
890     if (pickup_vehicle < 0 && delivery_vehicle < 0) {
891       pairs_to_insert_by_bucket[GetBucketOfPair(index_pair)].push_back(index);
892     }
893     if (pickup_vehicle >= 0 && delivery_vehicle < 0) {
894       std::vector<int>& pair_nodes = vehicle_to_pair_nodes[pickup_vehicle][1];
895       for (int64_t delivery : index_pair.second) {
896         pair_nodes.push_back(delivery);
897       }
898     }
899     if (pickup_vehicle < 0 && delivery_vehicle >= 0) {
900       std::vector<int>& pair_nodes = vehicle_to_pair_nodes[delivery_vehicle][1];
901       for (int64_t pickup : index_pair.first) {
902         pair_nodes.push_back(pickup);
903       }
904     }
905   }
906   for (const auto& [vehicle, nodes] : vehicle_to_pair_nodes) {
907     if (!InsertNodesOnRoutes(nodes, {vehicle})) return false;
908   }
909 
910   if (!InsertPairsAndNodesByRequirementTopologicalOrder()) return false;
911 
912   // TODO(user): Adapt the pair insertions to also support seed and
913   // sequential insertion.
914   if (!InsertPairs(pairs_to_insert_by_bucket)) return false;
915   std::map<int64_t, std::vector<int>> nodes_by_bucket;
916   for (int node = 0; node < model()->Size(); ++node) {
917     if (!Contains(node) && model()->GetPickupIndexPairs(node).empty() &&
918         model()->GetDeliveryIndexPairs(node).empty()) {
919       nodes_by_bucket[GetBucketOfNode(node)].push_back(node);
920     }
921   }
922   InsertFarthestNodesAsSeeds();
923   if (gci_params_.is_sequential) {
924     if (!SequentialInsertNodes(nodes_by_bucket)) return false;
925   } else if (!InsertNodesOnRoutes(nodes_by_bucket, {})) {
926     return false;
927   }
928   MakeUnassignedNodesUnperformed();
929   DCHECK(CheckVehicleIndices());
930   return Commit();
931 }
932 
933 bool GlobalCheapestInsertionFilteredHeuristic::
InsertPairsAndNodesByRequirementTopologicalOrder()934     InsertPairsAndNodesByRequirementTopologicalOrder() {
935   const RoutingModel::IndexPairs& pickup_delivery_pairs =
936       model()->GetPickupAndDeliveryPairs();
937   for (const std::vector<int>& types :
938        model()->GetTopologicallySortedVisitTypes()) {
939     for (int type : types) {
940       std::map<int64_t, std::vector<int>> pairs_to_insert_by_bucket;
941       for (int index : model()->GetPairIndicesOfType(type)) {
942         pairs_to_insert_by_bucket[GetBucketOfPair(pickup_delivery_pairs[index])]
943             .push_back(index);
944       }
945       if (!InsertPairs(pairs_to_insert_by_bucket)) return false;
946       std::map<int64_t, std::vector<int>> nodes_by_bucket;
947       for (int node : model()->GetSingleNodesOfType(type)) {
948         nodes_by_bucket[GetBucketOfNode(node)].push_back(node);
949       }
950       if (!InsertNodesOnRoutes(nodes_by_bucket, {})) return false;
951     }
952   }
953   return true;
954 }
955 
InsertPairs(const std::map<int64_t,std::vector<int>> & pair_indices_by_bucket)956 bool GlobalCheapestInsertionFilteredHeuristic::InsertPairs(
957     const std::map<int64_t, std::vector<int>>& pair_indices_by_bucket) {
958   AdjustablePriorityQueue<PairEntry> priority_queue;
959   std::vector<PairEntries> pickup_to_entries;
960   std::vector<PairEntries> delivery_to_entries;
961   std::vector<int> pair_indices_to_insert;
962   for (const auto& [bucket, pair_indices] : pair_indices_by_bucket) {
963     pair_indices_to_insert.insert(pair_indices_to_insert.end(),
964                                   pair_indices.begin(), pair_indices.end());
965     if (!InitializePairPositions(pair_indices_to_insert, &priority_queue,
966                                  &pickup_to_entries, &delivery_to_entries)) {
967       return false;
968     }
969     while (!priority_queue.IsEmpty()) {
970       if (StopSearchAndCleanup(&priority_queue)) {
971         return false;
972       }
973       PairEntry* const entry = priority_queue.Top();
974       const int64_t pickup = entry->pickup_to_insert();
975       const int64_t delivery = entry->delivery_to_insert();
976       if (Contains(pickup) || Contains(delivery)) {
977         DeletePairEntry(entry, &priority_queue, &pickup_to_entries,
978                         &delivery_to_entries);
979         continue;
980       }
981 
982       const int entry_vehicle = entry->vehicle();
983       if (entry_vehicle == -1) {
984         // Pair is unperformed.
985         SetValue(pickup, pickup);
986         SetValue(delivery, delivery);
987         if (!Commit()) {
988           DeletePairEntry(entry, &priority_queue, &pickup_to_entries,
989                           &delivery_to_entries);
990         }
991         continue;
992       }
993 
994       // Pair is performed.
995       if (UseEmptyVehicleTypeCuratorForVehicle(entry_vehicle)) {
996         if (!InsertPairEntryUsingEmptyVehicleTypeCurator(
997                 pair_indices_to_insert, entry, &priority_queue,
998                 &pickup_to_entries, &delivery_to_entries)) {
999           return false;
1000         }
1001         // The entry corresponded to an insertion on an empty vehicle, which was
1002         // handled by the call to InsertPairEntryUsingEmptyVehicleTypeCurator().
1003         continue;
1004       }
1005 
1006       const int64_t pickup_insert_after = entry->pickup_insert_after();
1007       const int64_t pickup_insert_before = Value(pickup_insert_after);
1008       InsertBetween(pickup, pickup_insert_after, pickup_insert_before);
1009 
1010       const int64_t delivery_insert_after = entry->delivery_insert_after();
1011       const int64_t delivery_insert_before = (delivery_insert_after == pickup)
1012                                                  ? pickup_insert_before
1013                                                  : Value(delivery_insert_after);
1014       InsertBetween(delivery, delivery_insert_after, delivery_insert_before);
1015       if (Commit()) {
1016         if (!UpdateAfterPairInsertion(
1017                 pair_indices_to_insert, entry_vehicle, pickup,
1018                 pickup_insert_after, delivery, delivery_insert_after,
1019                 &priority_queue, &pickup_to_entries, &delivery_to_entries)) {
1020           return false;
1021         }
1022       } else {
1023         DeletePairEntry(entry, &priority_queue, &pickup_to_entries,
1024                         &delivery_to_entries);
1025       }
1026     }
1027     // In case all pairs could not be inserted, pushing uninserted ones to the
1028     // next bucket.
1029     const RoutingModel::IndexPairs& pickup_delivery_pairs =
1030         model()->GetPickupAndDeliveryPairs();
1031     pair_indices_to_insert.erase(
1032         std::remove_if(
1033             pair_indices_to_insert.begin(), pair_indices_to_insert.end(),
1034             [this, &pickup_delivery_pairs](int pair_index) {
1035               // Checking a pickup has been inserted is enough to make sure the
1036               // full pair has been inserted since a pickup cannot be inserted
1037               // without its delivery (and vice versa).
1038               for (int64_t pickup : pickup_delivery_pairs[pair_index].first) {
1039                 if (Contains(pickup)) return true;
1040               }
1041               return false;
1042             }),
1043         pair_indices_to_insert.end());
1044   }
1045   return true;
1046 }
1047 
1048 bool GlobalCheapestInsertionFilteredHeuristic::
InsertPairEntryUsingEmptyVehicleTypeCurator(const std::vector<int> & pair_indices,GlobalCheapestInsertionFilteredHeuristic::PairEntry * const pair_entry,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::PairEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * pickup_to_entries,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * delivery_to_entries)1049     InsertPairEntryUsingEmptyVehicleTypeCurator(
1050         const std::vector<int>& pair_indices,
1051         GlobalCheapestInsertionFilteredHeuristic::PairEntry* const pair_entry,
1052         AdjustablePriorityQueue<
1053             GlobalCheapestInsertionFilteredHeuristic::PairEntry>*
1054             priority_queue,
1055         std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1056             pickup_to_entries,
1057         std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1058             delivery_to_entries) {
1059   const int entry_vehicle = pair_entry->vehicle();
1060   DCHECK(UseEmptyVehicleTypeCuratorForVehicle(entry_vehicle));
1061 
1062   // Trying to insert on an empty vehicle.
1063   // As we only have one pair_entry per empty vehicle type, we try inserting on
1064   // all vehicles of this type with the same fixed cost, as they all have the
1065   // same insertion value.
1066   const int64_t pickup = pair_entry->pickup_to_insert();
1067   const int64_t delivery = pair_entry->delivery_to_insert();
1068   const int64_t entry_fixed_cost =
1069       model()->GetFixedCostOfVehicle(entry_vehicle);
1070   auto vehicle_is_compatible = [this, entry_fixed_cost, pickup,
1071                                 delivery](int vehicle) {
1072     if (model()->GetFixedCostOfVehicle(vehicle) != entry_fixed_cost) {
1073       return false;
1074     }
1075     // NOTE: Only empty vehicles should be in the vehicle_curator_.
1076     DCHECK(VehicleIsEmpty(vehicle));
1077     const int64_t end = model()->End(vehicle);
1078     InsertBetween(pickup, model()->Start(vehicle), end);
1079     InsertBetween(delivery, pickup, end);
1080     return Commit();
1081   };
1082   // Since the vehicles of the same type are sorted by increasing fixed
1083   // cost by the curator, we can stop as soon as a vehicle with a fixed cost
1084   // higher than the entry_fixed_cost is found which is empty, and adapt the
1085   // pair entry with this new vehicle.
1086   auto stop_and_return_vehicle = [this, entry_fixed_cost](int vehicle) {
1087     return model()->GetFixedCostOfVehicle(vehicle) > entry_fixed_cost;
1088   };
1089   const auto [compatible_vehicle, next_fixed_cost_empty_vehicle] =
1090       empty_vehicle_type_curator_->GetCompatibleVehicleOfType(
1091           empty_vehicle_type_curator_->Type(entry_vehicle),
1092           vehicle_is_compatible, stop_and_return_vehicle);
1093   if (compatible_vehicle >= 0) {
1094     // The pair was inserted on this vehicle.
1095     const int64_t vehicle_start = model()->Start(compatible_vehicle);
1096     const int num_previous_vehicle_entries =
1097         pickup_to_entries->at(vehicle_start).size() +
1098         delivery_to_entries->at(vehicle_start).size();
1099     if (!UpdateAfterPairInsertion(
1100             pair_indices, compatible_vehicle, pickup, vehicle_start, delivery,
1101             pickup, priority_queue, pickup_to_entries, delivery_to_entries)) {
1102       return false;
1103     }
1104     if (compatible_vehicle != entry_vehicle) {
1105       // The pair was inserted on another empty vehicle of the same type
1106       // and same fixed cost as entry_vehicle.
1107       // Since this vehicle is empty and has the same fixed cost as the
1108       // entry_vehicle, it shouldn't be the representative of empty vehicles
1109       // for any pickup/delivery in the priority queue.
1110       DCHECK_EQ(num_previous_vehicle_entries, 0);
1111       return true;
1112     }
1113     // The previously unused entry_vehicle is now used, so we use the next
1114     // available vehicle of the same type to compute and store insertions on
1115     // empty vehicles.
1116     const int new_empty_vehicle =
1117         empty_vehicle_type_curator_->GetLowestFixedCostVehicleOfType(
1118             empty_vehicle_type_curator_->Type(compatible_vehicle));
1119 
1120     if (new_empty_vehicle >= 0) {
1121       DCHECK(VehicleIsEmpty(new_empty_vehicle));
1122       // Add node entries after this vehicle start for uninserted pairs which
1123       // don't have entries on this empty vehicle.
1124       UpdatePairPositions(pair_indices, new_empty_vehicle,
1125                           model()->Start(new_empty_vehicle), priority_queue,
1126                           pickup_to_entries, delivery_to_entries);
1127     }
1128   } else if (next_fixed_cost_empty_vehicle >= 0) {
1129     // Could not insert on this vehicle or any other vehicle of the same type
1130     // with the same fixed cost, but found an empty vehicle of this type with
1131     // higher fixed cost.
1132     DCHECK(VehicleIsEmpty(next_fixed_cost_empty_vehicle));
1133     // Update the pair entry to correspond to an insertion on this
1134     // next_fixed_cost_empty_vehicle instead of the previous entry_vehicle.
1135     pair_entry->set_vehicle(next_fixed_cost_empty_vehicle);
1136     pickup_to_entries->at(pair_entry->pickup_insert_after()).erase(pair_entry);
1137     pair_entry->set_pickup_insert_after(
1138         model()->Start(next_fixed_cost_empty_vehicle));
1139     pickup_to_entries->at(pair_entry->pickup_insert_after()).insert(pair_entry);
1140     DCHECK_EQ(pair_entry->delivery_insert_after(), pickup);
1141     UpdatePairEntry(pair_entry, priority_queue);
1142   } else {
1143     DeletePairEntry(pair_entry, priority_queue, pickup_to_entries,
1144                     delivery_to_entries);
1145   }
1146 
1147   return true;
1148 }
1149 
InsertNodesOnRoutes(const std::map<int64_t,std::vector<int>> & nodes_by_bucket,const absl::flat_hash_set<int> & vehicles)1150 bool GlobalCheapestInsertionFilteredHeuristic::InsertNodesOnRoutes(
1151     const std::map<int64_t, std::vector<int>>& nodes_by_bucket,
1152     const absl::flat_hash_set<int>& vehicles) {
1153   AdjustablePriorityQueue<NodeEntry> priority_queue;
1154   std::vector<NodeEntries> position_to_node_entries;
1155   std::vector<int> nodes_to_insert;
1156   for (const auto& [bucket, nodes] : nodes_by_bucket) {
1157     nodes_to_insert.insert(nodes_to_insert.end(), nodes.begin(), nodes.end());
1158     if (!InitializePositions(nodes_to_insert, vehicles, &priority_queue,
1159                              &position_to_node_entries)) {
1160       return false;
1161     }
1162     // The following boolean indicates whether or not all vehicles are being
1163     // considered for insertion of the nodes simultaneously.
1164     // In the sequential version of the heuristic, as well as when inserting
1165     // single pickup or deliveries from pickup/delivery pairs, this will be
1166     // false. In the general parallel version of the heuristic, all_vehicles is
1167     // true.
1168     const bool all_vehicles =
1169         vehicles.empty() || vehicles.size() == model()->vehicles();
1170 
1171     while (!priority_queue.IsEmpty()) {
1172       NodeEntry* const node_entry = priority_queue.Top();
1173       if (StopSearchAndCleanup(&priority_queue)) {
1174         return false;
1175       }
1176       const int64_t node_to_insert = node_entry->node_to_insert();
1177       if (Contains(node_to_insert)) {
1178         DeleteNodeEntry(node_entry, &priority_queue, &position_to_node_entries);
1179         continue;
1180       }
1181 
1182       const int entry_vehicle = node_entry->vehicle();
1183       if (entry_vehicle == -1) {
1184         DCHECK(all_vehicles);
1185         // Make node unperformed.
1186         SetValue(node_to_insert, node_to_insert);
1187         if (!Commit()) {
1188           DeleteNodeEntry(node_entry, &priority_queue,
1189                           &position_to_node_entries);
1190         }
1191         continue;
1192       }
1193 
1194       // Make node performed.
1195       if (UseEmptyVehicleTypeCuratorForVehicle(entry_vehicle, all_vehicles)) {
1196         DCHECK(all_vehicles);
1197         if (!InsertNodeEntryUsingEmptyVehicleTypeCurator(
1198                 nodes_to_insert, all_vehicles, node_entry, &priority_queue,
1199                 &position_to_node_entries)) {
1200           return false;
1201         }
1202         continue;
1203       }
1204 
1205       const int64_t insert_after = node_entry->insert_after();
1206       InsertBetween(node_to_insert, insert_after, Value(insert_after));
1207       if (Commit()) {
1208         if (!UpdatePositions(nodes_to_insert, entry_vehicle, node_to_insert,
1209                              all_vehicles, &priority_queue,
1210                              &position_to_node_entries) ||
1211             !UpdatePositions(nodes_to_insert, entry_vehicle, insert_after,
1212                              all_vehicles, &priority_queue,
1213                              &position_to_node_entries)) {
1214           return false;
1215         }
1216         SetVehicleIndex(node_to_insert, entry_vehicle);
1217       } else {
1218         DeleteNodeEntry(node_entry, &priority_queue, &position_to_node_entries);
1219       }
1220     }
1221     // In case all nodes could not be inserted, pushing uninserted ones to the
1222     // next bucket.
1223     nodes_to_insert.erase(
1224         std::remove_if(nodes_to_insert.begin(), nodes_to_insert.end(),
1225                        [this](int node) { return Contains(node); }),
1226         nodes_to_insert.end());
1227   }
1228   return true;
1229 }
1230 
1231 bool GlobalCheapestInsertionFilteredHeuristic::
InsertNodeEntryUsingEmptyVehicleTypeCurator(const std::vector<int> & nodes,bool all_vehicles,GlobalCheapestInsertionFilteredHeuristic::NodeEntry * const node_entry,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::NodeEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::NodeEntries> * position_to_node_entries)1232     InsertNodeEntryUsingEmptyVehicleTypeCurator(
1233         const std::vector<int>& nodes, bool all_vehicles,
1234         GlobalCheapestInsertionFilteredHeuristic::NodeEntry* const node_entry,
1235         AdjustablePriorityQueue<
1236             GlobalCheapestInsertionFilteredHeuristic::NodeEntry>*
1237             priority_queue,
1238         std::vector<GlobalCheapestInsertionFilteredHeuristic::NodeEntries>*
1239             position_to_node_entries) {
1240   const int entry_vehicle = node_entry->vehicle();
1241   DCHECK(UseEmptyVehicleTypeCuratorForVehicle(entry_vehicle, all_vehicles));
1242 
1243   // Trying to insert on an empty vehicle, and all vehicles are being
1244   // considered simultaneously.
1245   // As we only have one node_entry per type, we try inserting on all vehicles
1246   // of this type with the same fixed cost as they all have the same insertion
1247   // value.
1248   const int64_t node_to_insert = node_entry->node_to_insert();
1249   const int64_t entry_fixed_cost =
1250       model()->GetFixedCostOfVehicle(entry_vehicle);
1251   auto vehicle_is_compatible = [this, entry_fixed_cost,
1252                                 node_to_insert](int vehicle) {
1253     if (model()->GetFixedCostOfVehicle(vehicle) != entry_fixed_cost) {
1254       return false;
1255     }
1256     // NOTE: Only empty vehicles should be in the vehicle_curator_.
1257     DCHECK(VehicleIsEmpty(vehicle));
1258     InsertBetween(node_to_insert, model()->Start(vehicle),
1259                   model()->End(vehicle));
1260     return Commit();
1261   };
1262   // Since the vehicles of the same type are sorted by increasing fixed
1263   // cost by the curator, we can stop as soon as an empty vehicle with a fixed
1264   // cost higher than the entry_fixed_cost is found, and add new entries for
1265   // this new vehicle.
1266   auto stop_and_return_vehicle = [this, entry_fixed_cost](int vehicle) {
1267     return model()->GetFixedCostOfVehicle(vehicle) > entry_fixed_cost;
1268   };
1269   const auto [compatible_vehicle, next_fixed_cost_empty_vehicle] =
1270       empty_vehicle_type_curator_->GetCompatibleVehicleOfType(
1271           empty_vehicle_type_curator_->Type(entry_vehicle),
1272           vehicle_is_compatible, stop_and_return_vehicle);
1273   if (compatible_vehicle >= 0) {
1274     // The node was inserted on this vehicle.
1275     if (!UpdatePositions(nodes, compatible_vehicle, node_to_insert,
1276                          all_vehicles, priority_queue,
1277                          position_to_node_entries)) {
1278       return false;
1279     }
1280     const int64_t compatible_start = model()->Start(compatible_vehicle);
1281     const bool no_prior_entries_for_this_vehicle =
1282         position_to_node_entries->at(compatible_start).empty();
1283     if (!UpdatePositions(nodes, compatible_vehicle, compatible_start,
1284                          all_vehicles, priority_queue,
1285                          position_to_node_entries)) {
1286       return false;
1287     }
1288     SetVehicleIndex(node_to_insert, compatible_vehicle);
1289     if (compatible_vehicle != entry_vehicle) {
1290       // The node was inserted on another empty vehicle of the same type
1291       // and same fixed cost as entry_vehicle.
1292       // Since this vehicle is empty and has the same fixed cost as the
1293       // entry_vehicle, it shouldn't be the representative of empty vehicles
1294       // for any node in the priority queue.
1295       DCHECK(no_prior_entries_for_this_vehicle);
1296       return true;
1297     }
1298     // The previously unused entry_vehicle is now used, so we use the next
1299     // available vehicle of the same type to compute and store insertions on
1300     // empty vehicles.
1301     const int new_empty_vehicle =
1302         empty_vehicle_type_curator_->GetLowestFixedCostVehicleOfType(
1303             empty_vehicle_type_curator_->Type(compatible_vehicle));
1304 
1305     if (new_empty_vehicle >= 0) {
1306       DCHECK(VehicleIsEmpty(new_empty_vehicle));
1307       // Add node entries after this vehicle start for uninserted nodes which
1308       // don't have entries on this empty vehicle.
1309       if (!UpdatePositions(nodes, new_empty_vehicle,
1310                            model()->Start(new_empty_vehicle), all_vehicles,
1311                            priority_queue, position_to_node_entries)) {
1312         return false;
1313       }
1314     }
1315   } else if (next_fixed_cost_empty_vehicle >= 0) {
1316     // Could not insert on this vehicle or any other vehicle of the same
1317     // type with the same fixed cost, but found an empty vehicle of this type
1318     // with higher fixed cost.
1319     DCHECK(VehicleIsEmpty(next_fixed_cost_empty_vehicle));
1320     // Update the insertion entry to be on next_empty_vehicle instead of the
1321     // previous entry_vehicle.
1322     position_to_node_entries->at(node_entry->insert_after()).erase(node_entry);
1323     node_entry->set_insert_after(model()->Start(next_fixed_cost_empty_vehicle));
1324     position_to_node_entries->at(node_entry->insert_after()).insert(node_entry);
1325     node_entry->set_vehicle(next_fixed_cost_empty_vehicle);
1326     UpdateNodeEntry(node_entry, priority_queue);
1327   } else {
1328     DeleteNodeEntry(node_entry, priority_queue, position_to_node_entries);
1329   }
1330 
1331   return true;
1332 }
1333 
SequentialInsertNodes(const std::map<int64_t,std::vector<int>> & nodes_by_bucket)1334 bool GlobalCheapestInsertionFilteredHeuristic::SequentialInsertNodes(
1335     const std::map<int64_t, std::vector<int>>& nodes_by_bucket) {
1336   std::vector<bool> is_vehicle_used;
1337   absl::flat_hash_set<int> used_vehicles;
1338   std::vector<int> unused_vehicles;
1339 
1340   DetectUsedVehicles(&is_vehicle_used, &unused_vehicles, &used_vehicles);
1341   if (!used_vehicles.empty() &&
1342       !InsertNodesOnRoutes(nodes_by_bucket, used_vehicles)) {
1343     return false;
1344   }
1345 
1346   std::vector<std::vector<StartEndValue>> start_end_distances_per_node =
1347       ComputeStartEndDistanceForVehicles(unused_vehicles);
1348   std::priority_queue<Seed, std::vector<Seed>, std::greater<Seed>>
1349       first_node_queue;
1350   InitializePriorityQueue(&start_end_distances_per_node, &first_node_queue);
1351 
1352   int vehicle = InsertSeedNode(&start_end_distances_per_node, &first_node_queue,
1353                                &is_vehicle_used);
1354 
1355   while (vehicle >= 0) {
1356     if (!InsertNodesOnRoutes(nodes_by_bucket, {vehicle})) {
1357       return false;
1358     }
1359     vehicle = InsertSeedNode(&start_end_distances_per_node, &first_node_queue,
1360                              &is_vehicle_used);
1361   }
1362   return true;
1363 }
1364 
DetectUsedVehicles(std::vector<bool> * is_vehicle_used,std::vector<int> * unused_vehicles,absl::flat_hash_set<int> * used_vehicles)1365 void GlobalCheapestInsertionFilteredHeuristic::DetectUsedVehicles(
1366     std::vector<bool>* is_vehicle_used, std::vector<int>* unused_vehicles,
1367     absl::flat_hash_set<int>* used_vehicles) {
1368   is_vehicle_used->clear();
1369   is_vehicle_used->resize(model()->vehicles());
1370 
1371   used_vehicles->clear();
1372   used_vehicles->reserve(model()->vehicles());
1373 
1374   unused_vehicles->clear();
1375   unused_vehicles->reserve(model()->vehicles());
1376 
1377   for (int vehicle = 0; vehicle < model()->vehicles(); vehicle++) {
1378     if (!VehicleIsEmpty(vehicle)) {
1379       (*is_vehicle_used)[vehicle] = true;
1380       used_vehicles->insert(vehicle);
1381     } else {
1382       (*is_vehicle_used)[vehicle] = false;
1383       unused_vehicles->push_back(vehicle);
1384     }
1385   }
1386 }
1387 
InsertFarthestNodesAsSeeds()1388 void GlobalCheapestInsertionFilteredHeuristic::InsertFarthestNodesAsSeeds() {
1389   if (gci_params_.farthest_seeds_ratio <= 0) return;
1390   // Insert at least 1 farthest Seed if the parameter is positive.
1391   const int num_seeds = static_cast<int>(
1392       std::ceil(gci_params_.farthest_seeds_ratio * model()->vehicles()));
1393 
1394   std::vector<bool> is_vehicle_used;
1395   absl::flat_hash_set<int> used_vehicles;
1396   std::vector<int> unused_vehicles;
1397   DetectUsedVehicles(&is_vehicle_used, &unused_vehicles, &used_vehicles);
1398   std::vector<std::vector<StartEndValue>> start_end_distances_per_node =
1399       ComputeStartEndDistanceForVehicles(unused_vehicles);
1400 
1401   // Priority queue where the Seeds with a larger distance are given higher
1402   // priority.
1403   std::priority_queue<Seed> farthest_node_queue;
1404   InitializePriorityQueue(&start_end_distances_per_node, &farthest_node_queue);
1405 
1406   int inserted_seeds = 0;
1407   while (inserted_seeds++ < num_seeds) {
1408     if (InsertSeedNode(&start_end_distances_per_node, &farthest_node_queue,
1409                        &is_vehicle_used) < 0) {
1410       break;
1411     }
1412   }
1413 
1414   // NOTE: As we don't use the empty_vehicle_type_curator_ when inserting seed
1415   // nodes on routes, some previously empty vehicles may now be used, so we
1416   // update the curator accordingly to ensure it still only stores empty
1417   // vehicles.
1418   DCHECK(empty_vehicle_type_curator_ != nullptr);
1419   empty_vehicle_type_curator_->Update(
1420       [this](int vehicle) { return !VehicleIsEmpty(vehicle); });
1421 }
1422 
1423 template <class Queue>
InsertSeedNode(std::vector<std::vector<StartEndValue>> * start_end_distances_per_node,Queue * priority_queue,std::vector<bool> * is_vehicle_used)1424 int GlobalCheapestInsertionFilteredHeuristic::InsertSeedNode(
1425     std::vector<std::vector<StartEndValue>>* start_end_distances_per_node,
1426     Queue* priority_queue, std::vector<bool>* is_vehicle_used) {
1427   while (!priority_queue->empty()) {
1428     if (StopSearch()) return -1;
1429     const Seed& seed = priority_queue->top();
1430 
1431     const int seed_node = seed.second;
1432     const int seed_vehicle = seed.first.vehicle;
1433 
1434     std::vector<StartEndValue>& other_start_end_values =
1435         (*start_end_distances_per_node)[seed_node];
1436 
1437     if (Contains(seed_node)) {
1438       // The node is already inserted, it is therefore no longer considered as
1439       // a potential seed.
1440       priority_queue->pop();
1441       other_start_end_values.clear();
1442       continue;
1443     }
1444     if (!(*is_vehicle_used)[seed_vehicle]) {
1445       // Try to insert this seed_node on this vehicle's route.
1446       const int64_t start = model()->Start(seed_vehicle);
1447       const int64_t end = model()->End(seed_vehicle);
1448       DCHECK_EQ(Value(start), end);
1449       InsertBetween(seed_node, start, end);
1450       if (Commit()) {
1451         priority_queue->pop();
1452         (*is_vehicle_used)[seed_vehicle] = true;
1453         other_start_end_values.clear();
1454         SetVehicleIndex(seed_node, seed_vehicle);
1455         return seed_vehicle;
1456       }
1457     }
1458     // Either the vehicle is already used, or the Commit() wasn't successful.
1459     // In both cases, we remove this Seed from the priority queue, and insert
1460     // the next StartEndValue from start_end_distances_per_node[seed_node]
1461     // in the priority queue.
1462     priority_queue->pop();
1463     if (!other_start_end_values.empty()) {
1464       const StartEndValue& next_seed_value = other_start_end_values.back();
1465       priority_queue->push(std::make_pair(next_seed_value, seed_node));
1466       other_start_end_values.pop_back();
1467     }
1468   }
1469   // No seed node was inserted.
1470   return -1;
1471 }
1472 
InitializePairPositions(const std::vector<int> & pair_indices,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::PairEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * pickup_to_entries,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * delivery_to_entries)1473 bool GlobalCheapestInsertionFilteredHeuristic::InitializePairPositions(
1474     const std::vector<int>& pair_indices,
1475     AdjustablePriorityQueue<
1476         GlobalCheapestInsertionFilteredHeuristic::PairEntry>* priority_queue,
1477     std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1478         pickup_to_entries,
1479     std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1480         delivery_to_entries) {
1481   priority_queue->Clear();
1482   pickup_to_entries->clear();
1483   pickup_to_entries->resize(model()->Size());
1484   delivery_to_entries->clear();
1485   delivery_to_entries->resize(model()->Size());
1486   const RoutingModel::IndexPairs& pickup_delivery_pairs =
1487       model()->GetPickupAndDeliveryPairs();
1488   for (int index : pair_indices) {
1489     const RoutingModel::IndexPair& index_pair = pickup_delivery_pairs[index];
1490     for (int64_t pickup : index_pair.first) {
1491       if (Contains(pickup)) continue;
1492       for (int64_t delivery : index_pair.second) {
1493         if (Contains(delivery)) continue;
1494         if (StopSearchAndCleanup(priority_queue)) return false;
1495         // Add insertion entry making pair unperformed. When the pair is part
1496         // of a disjunction we do not try to make any of its pairs unperformed
1497         // as it requires having an entry with all pairs being unperformed.
1498         // TODO(user): Adapt the code to make pair disjunctions unperformed.
1499         if (gci_params_.add_unperformed_entries &&
1500             index_pair.first.size() == 1 && index_pair.second.size() == 1 &&
1501             GetUnperformedValue(pickup) !=
1502                 std::numeric_limits<int64_t>::max() &&
1503             GetUnperformedValue(delivery) !=
1504                 std::numeric_limits<int64_t>::max()) {
1505           AddPairEntry(pickup, -1, delivery, -1, -1, priority_queue, nullptr,
1506                        nullptr);
1507         }
1508         // Add all other insertion entries with pair performed.
1509         InitializeInsertionEntriesPerformingPair(
1510             pickup, delivery, priority_queue, pickup_to_entries,
1511             delivery_to_entries);
1512       }
1513     }
1514   }
1515   return true;
1516 }
1517 
1518 void GlobalCheapestInsertionFilteredHeuristic::
InitializeInsertionEntriesPerformingPair(int64_t pickup,int64_t delivery,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::PairEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * pickup_to_entries,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * delivery_to_entries)1519     InitializeInsertionEntriesPerformingPair(
1520         int64_t pickup, int64_t delivery,
1521         AdjustablePriorityQueue<
1522             GlobalCheapestInsertionFilteredHeuristic::PairEntry>*
1523             priority_queue,
1524         std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1525             pickup_to_entries,
1526         std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1527             delivery_to_entries) {
1528   if (!gci_params_.use_neighbors_ratio_for_initialization) {
1529     struct PairInsertion {
1530       int64_t insert_pickup_after;
1531       int64_t insert_delivery_after;
1532       int vehicle;
1533     };
1534     std::vector<PairInsertion> pair_insertions;
1535     for (int vehicle = 0; vehicle < model()->vehicles(); ++vehicle) {
1536       if (VehicleIsEmpty(vehicle) &&
1537           empty_vehicle_type_curator_->GetLowestFixedCostVehicleOfType(
1538               empty_vehicle_type_curator_->Type(vehicle)) != vehicle) {
1539         // We only consider the least expensive empty vehicle of each type for
1540         // entries.
1541         continue;
1542       }
1543       const int64_t start = model()->Start(vehicle);
1544       std::vector<NodeInsertion> pickup_insertions;
1545       AppendInsertionPositionsAfter(pickup, start, Value(start), vehicle,
1546                                     &pickup_insertions);
1547       for (const auto [insert_pickup_after, pickup_vehicle,
1548                        unused_pickup_value] : pickup_insertions) {
1549         CHECK(!model()->IsEnd(insert_pickup_after));
1550         std::vector<NodeInsertion> delivery_insertions;
1551         AppendInsertionPositionsAfter(delivery, pickup,
1552                                       Value(insert_pickup_after), vehicle,
1553                                       &delivery_insertions);
1554         for (const auto [insert_delivery_after, delivery_vehicle,
1555                          unused_delivery_value] : delivery_insertions) {
1556           pair_insertions.push_back(
1557               {insert_pickup_after, insert_delivery_after, vehicle});
1558         }
1559       }
1560     }
1561     for (const auto& [insert_pickup_after, insert_delivery_after, vehicle] :
1562          pair_insertions) {
1563       DCHECK_NE(insert_pickup_after, insert_delivery_after);
1564       AddPairEntry(pickup, insert_pickup_after, delivery, insert_delivery_after,
1565                    vehicle, priority_queue, pickup_to_entries,
1566                    delivery_to_entries);
1567     }
1568     return;
1569   }
1570 
1571   // We're only considering the closest neighbors as insertion positions for
1572   // the pickup/delivery pair.
1573   for (int cost_class = 0; cost_class < model()->GetCostClassesCount();
1574        cost_class++) {
1575     absl::flat_hash_set<std::pair<int64_t, int64_t>>
1576         existing_insertion_positions;
1577     // Explore the neighborhood of the pickup.
1578     for (const int64_t pickup_insert_after :
1579          GetNeighborsOfNodeForCostClass(cost_class, pickup)) {
1580       if (!Contains(pickup_insert_after)) {
1581         continue;
1582       }
1583       const int vehicle = node_index_to_vehicle_[pickup_insert_after];
1584       if (vehicle < 0 ||
1585           model()->GetCostClassIndexOfVehicle(vehicle).value() != cost_class) {
1586         continue;
1587       }
1588 
1589       if (VehicleIsEmpty(vehicle) &&
1590           empty_vehicle_type_curator_->GetLowestFixedCostVehicleOfType(
1591               empty_vehicle_type_curator_->Type(vehicle)) != vehicle) {
1592         // We only consider the least expensive empty vehicle of each type for
1593         // entries.
1594         continue;
1595       }
1596 
1597       int64_t delivery_insert_after = pickup;
1598       while (!model()->IsEnd(delivery_insert_after)) {
1599         const std::pair<int64_t, int64_t> insertion_position = {
1600             pickup_insert_after, delivery_insert_after};
1601         DCHECK(!existing_insertion_positions.contains(insertion_position));
1602         existing_insertion_positions.insert(insertion_position);
1603 
1604         AddPairEntry(pickup, pickup_insert_after, delivery,
1605                      delivery_insert_after, vehicle, priority_queue,
1606                      pickup_to_entries, delivery_to_entries);
1607         delivery_insert_after = (delivery_insert_after == pickup)
1608                                     ? Value(pickup_insert_after)
1609                                     : Value(delivery_insert_after);
1610       }
1611     }
1612 
1613     // Explore the neighborhood of the delivery.
1614     for (const int64_t delivery_insert_after :
1615          GetNeighborsOfNodeForCostClass(cost_class, delivery)) {
1616       if (!Contains(delivery_insert_after)) {
1617         continue;
1618       }
1619       const int vehicle = node_index_to_vehicle_[delivery_insert_after];
1620       if (vehicle < 0 ||
1621           model()->GetCostClassIndexOfVehicle(vehicle).value() != cost_class) {
1622         continue;
1623       }
1624 
1625       if (VehicleIsEmpty(vehicle)) {
1626         // Vehicle is empty.
1627         DCHECK_EQ(delivery_insert_after, model()->Start(vehicle));
1628       }
1629 
1630       int64_t pickup_insert_after = model()->Start(vehicle);
1631       while (pickup_insert_after != delivery_insert_after) {
1632         if (!existing_insertion_positions.contains(
1633                 std::make_pair(pickup_insert_after, delivery_insert_after))) {
1634           AddPairEntry(pickup, pickup_insert_after, delivery,
1635                        delivery_insert_after, vehicle, priority_queue,
1636                        pickup_to_entries, delivery_to_entries);
1637         }
1638         pickup_insert_after = Value(pickup_insert_after);
1639       }
1640     }
1641   }
1642 }
1643 
UpdateAfterPairInsertion(const std::vector<int> & pair_indices,int vehicle,int64_t pickup,int64_t pickup_position,int64_t delivery,int64_t delivery_position,AdjustablePriorityQueue<PairEntry> * priority_queue,std::vector<PairEntries> * pickup_to_entries,std::vector<PairEntries> * delivery_to_entries)1644 bool GlobalCheapestInsertionFilteredHeuristic::UpdateAfterPairInsertion(
1645     const std::vector<int>& pair_indices, int vehicle, int64_t pickup,
1646     int64_t pickup_position, int64_t delivery, int64_t delivery_position,
1647     AdjustablePriorityQueue<PairEntry>* priority_queue,
1648     std::vector<PairEntries>* pickup_to_entries,
1649     std::vector<PairEntries>* delivery_to_entries) {
1650   if (!UpdatePairPositions(pair_indices, vehicle, pickup_position,
1651                            priority_queue, pickup_to_entries,
1652                            delivery_to_entries) ||
1653       !UpdatePairPositions(pair_indices, vehicle, pickup, priority_queue,
1654                            pickup_to_entries, delivery_to_entries) ||
1655       !UpdatePairPositions(pair_indices, vehicle, delivery, priority_queue,
1656                            pickup_to_entries, delivery_to_entries)) {
1657     return false;
1658   }
1659   if (delivery_position != pickup &&
1660       !UpdatePairPositions(pair_indices, vehicle, delivery_position,
1661                            priority_queue, pickup_to_entries,
1662                            delivery_to_entries)) {
1663     return false;
1664   }
1665   SetVehicleIndex(pickup, vehicle);
1666   SetVehicleIndex(delivery, vehicle);
1667   return true;
1668 }
1669 
UpdatePickupPositions(const std::vector<int> & pair_indices,int vehicle,int64_t pickup_insert_after,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::PairEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * pickup_to_entries,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * delivery_to_entries)1670 bool GlobalCheapestInsertionFilteredHeuristic::UpdatePickupPositions(
1671     const std::vector<int>& pair_indices, int vehicle,
1672     int64_t pickup_insert_after,
1673     AdjustablePriorityQueue<
1674         GlobalCheapestInsertionFilteredHeuristic::PairEntry>* priority_queue,
1675     std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1676         pickup_to_entries,
1677     std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1678         delivery_to_entries) {
1679   // First, remove entries which have already been inserted and keep track of
1680   // the entries which are being kept and must be updated.
1681   using Pair = std::pair<int64_t, int64_t>;
1682   using Insertion = std::pair<Pair, /*delivery_insert_after*/ int64_t>;
1683   absl::flat_hash_set<Insertion> existing_insertions;
1684   std::vector<PairEntry*> to_remove;
1685   for (PairEntry* const pair_entry :
1686        pickup_to_entries->at(pickup_insert_after)) {
1687     DCHECK(priority_queue->Contains(pair_entry));
1688     DCHECK_EQ(pair_entry->pickup_insert_after(), pickup_insert_after);
1689     if (Contains(pair_entry->pickup_to_insert()) ||
1690         Contains(pair_entry->delivery_to_insert())) {
1691       to_remove.push_back(pair_entry);
1692     } else {
1693       DCHECK(delivery_to_entries->at(pair_entry->delivery_insert_after())
1694                  .contains(pair_entry));
1695       UpdatePairEntry(pair_entry, priority_queue);
1696       existing_insertions.insert(
1697           {{pair_entry->pickup_to_insert(), pair_entry->delivery_to_insert()},
1698            pair_entry->delivery_insert_after()});
1699     }
1700   }
1701   for (PairEntry* const pair_entry : to_remove) {
1702     DeletePairEntry(pair_entry, priority_queue, pickup_to_entries,
1703                     delivery_to_entries);
1704   }
1705   // Create new entries for which the pickup is to be inserted after
1706   // pickup_insert_after.
1707   const int cost_class = model()->GetCostClassIndexOfVehicle(vehicle).value();
1708   const int64_t pickup_insert_before = Value(pickup_insert_after);
1709   const RoutingModel::IndexPairs& pickup_delivery_pairs =
1710       model()->GetPickupAndDeliveryPairs();
1711   for (int pair_index : pair_indices) {
1712     if (StopSearchAndCleanup(priority_queue)) return false;
1713     const RoutingModel::IndexPair& index_pair =
1714         pickup_delivery_pairs[pair_index];
1715     for (int64_t pickup : index_pair.first) {
1716       if (Contains(pickup) ||
1717           !IsNeighborForCostClass(cost_class, pickup_insert_after, pickup)) {
1718         continue;
1719       }
1720       for (int64_t delivery : index_pair.second) {
1721         if (Contains(delivery)) {
1722           continue;
1723         }
1724         int64_t delivery_insert_after = pickup;
1725         while (!model()->IsEnd(delivery_insert_after)) {
1726           const Insertion insertion = {{pickup, delivery},
1727                                        delivery_insert_after};
1728           if (!existing_insertions.contains(insertion)) {
1729             AddPairEntry(pickup, pickup_insert_after, delivery,
1730                          delivery_insert_after, vehicle, priority_queue,
1731                          pickup_to_entries, delivery_to_entries);
1732           }
1733           if (delivery_insert_after == pickup) {
1734             delivery_insert_after = pickup_insert_before;
1735           } else {
1736             delivery_insert_after = Value(delivery_insert_after);
1737           }
1738         }
1739       }
1740     }
1741   }
1742   return true;
1743 }
1744 
UpdateDeliveryPositions(const std::vector<int> & pair_indices,int vehicle,int64_t delivery_insert_after,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::PairEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * pickup_to_entries,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * delivery_to_entries)1745 bool GlobalCheapestInsertionFilteredHeuristic::UpdateDeliveryPositions(
1746     const std::vector<int>& pair_indices, int vehicle,
1747     int64_t delivery_insert_after,
1748     AdjustablePriorityQueue<
1749         GlobalCheapestInsertionFilteredHeuristic::PairEntry>* priority_queue,
1750     std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1751         pickup_to_entries,
1752     std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1753         delivery_to_entries) {
1754   // First, remove entries which have already been inserted and keep track of
1755   // the entries which are being kept and must be updated.
1756   using Pair = std::pair<int64_t, int64_t>;
1757   using Insertion = std::pair<Pair, /*pickup_insert_after*/ int64_t>;
1758   absl::flat_hash_set<Insertion> existing_insertions;
1759   std::vector<PairEntry*> to_remove;
1760   for (PairEntry* const pair_entry :
1761        delivery_to_entries->at(delivery_insert_after)) {
1762     DCHECK(priority_queue->Contains(pair_entry));
1763     DCHECK_EQ(pair_entry->delivery_insert_after(), delivery_insert_after);
1764     if (Contains(pair_entry->pickup_to_insert()) ||
1765         Contains(pair_entry->delivery_to_insert())) {
1766       to_remove.push_back(pair_entry);
1767     } else {
1768       DCHECK(pickup_to_entries->at(pair_entry->pickup_insert_after())
1769                  .contains(pair_entry));
1770       existing_insertions.insert(
1771           {{pair_entry->pickup_to_insert(), pair_entry->delivery_to_insert()},
1772            pair_entry->pickup_insert_after()});
1773       UpdatePairEntry(pair_entry, priority_queue);
1774     }
1775   }
1776   for (PairEntry* const pair_entry : to_remove) {
1777     DeletePairEntry(pair_entry, priority_queue, pickup_to_entries,
1778                     delivery_to_entries);
1779   }
1780   // Create new entries for which the delivery is to be inserted after
1781   // delivery_insert_after.
1782   const int cost_class = model()->GetCostClassIndexOfVehicle(vehicle).value();
1783   const RoutingModel::IndexPairs& pickup_delivery_pairs =
1784       model()->GetPickupAndDeliveryPairs();
1785   for (int pair_index : pair_indices) {
1786     if (StopSearchAndCleanup(priority_queue)) return false;
1787     const RoutingModel::IndexPair& index_pair =
1788         pickup_delivery_pairs[pair_index];
1789     for (int64_t delivery : index_pair.second) {
1790       if (Contains(delivery) ||
1791           !IsNeighborForCostClass(cost_class, delivery_insert_after,
1792                                   delivery)) {
1793         continue;
1794       }
1795       for (int64_t pickup : index_pair.first) {
1796         if (Contains(pickup)) {
1797           continue;
1798         }
1799         int64_t pickup_insert_after = model()->Start(vehicle);
1800         while (pickup_insert_after != delivery_insert_after) {
1801           const Insertion insertion = {{pickup, delivery}, pickup_insert_after};
1802           if (!existing_insertions.contains(insertion)) {
1803             AddPairEntry(pickup, pickup_insert_after, delivery,
1804                          delivery_insert_after, vehicle, priority_queue,
1805                          pickup_to_entries, delivery_to_entries);
1806           }
1807           pickup_insert_after = Value(pickup_insert_after);
1808         }
1809       }
1810     }
1811   }
1812   return true;
1813 }
1814 
DeletePairEntry(GlobalCheapestInsertionFilteredHeuristic::PairEntry * entry,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::PairEntry> * priority_queue,std::vector<PairEntries> * pickup_to_entries,std::vector<PairEntries> * delivery_to_entries)1815 void GlobalCheapestInsertionFilteredHeuristic::DeletePairEntry(
1816     GlobalCheapestInsertionFilteredHeuristic::PairEntry* entry,
1817     AdjustablePriorityQueue<
1818         GlobalCheapestInsertionFilteredHeuristic::PairEntry>* priority_queue,
1819     std::vector<PairEntries>* pickup_to_entries,
1820     std::vector<PairEntries>* delivery_to_entries) {
1821   priority_queue->Remove(entry);
1822   if (entry->pickup_insert_after() != -1) {
1823     pickup_to_entries->at(entry->pickup_insert_after()).erase(entry);
1824   }
1825   if (entry->delivery_insert_after() != -1) {
1826     delivery_to_entries->at(entry->delivery_insert_after()).erase(entry);
1827   }
1828   delete entry;
1829 }
1830 
AddPairEntry(int64_t pickup,int64_t pickup_insert_after,int64_t delivery,int64_t delivery_insert_after,int vehicle,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::PairEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * pickup_entries,std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries> * delivery_entries) const1831 void GlobalCheapestInsertionFilteredHeuristic::AddPairEntry(
1832     int64_t pickup, int64_t pickup_insert_after, int64_t delivery,
1833     int64_t delivery_insert_after, int vehicle,
1834     AdjustablePriorityQueue<
1835         GlobalCheapestInsertionFilteredHeuristic::PairEntry>* priority_queue,
1836     std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1837         pickup_entries,
1838     std::vector<GlobalCheapestInsertionFilteredHeuristic::PairEntries>*
1839         delivery_entries) const {
1840   const IntVar* pickup_vehicle_var = model()->VehicleVar(pickup);
1841   const IntVar* delivery_vehicle_var = model()->VehicleVar(delivery);
1842   if (!pickup_vehicle_var->Contains(vehicle) ||
1843       !delivery_vehicle_var->Contains(vehicle)) {
1844     if (vehicle == -1 || !VehicleIsEmpty(vehicle)) return;
1845     // We need to check there is not an equivalent empty vehicle the pair
1846     // could fit on.
1847     const auto vehicle_is_compatible = [pickup_vehicle_var,
1848                                         delivery_vehicle_var](int vehicle) {
1849       return pickup_vehicle_var->Contains(vehicle) &&
1850              delivery_vehicle_var->Contains(vehicle);
1851     };
1852     if (!empty_vehicle_type_curator_->HasCompatibleVehicleOfType(
1853             empty_vehicle_type_curator_->Type(vehicle),
1854             vehicle_is_compatible)) {
1855       return;
1856     }
1857   }
1858   const int num_allowed_vehicles =
1859       std::min(pickup_vehicle_var->Size(), delivery_vehicle_var->Size());
1860   if (pickup_insert_after == -1) {
1861     DCHECK_EQ(delivery_insert_after, -1);
1862     DCHECK_EQ(vehicle, -1);
1863     PairEntry* pair_entry =
1864         new PairEntry(pickup, -1, delivery, -1, -1, num_allowed_vehicles);
1865     pair_entry->set_value(
1866         absl::GetFlag(FLAGS_routing_shift_insertion_cost_by_penalty)
1867             ? 0
1868             : CapAdd(GetUnperformedValue(pickup),
1869                      GetUnperformedValue(delivery)));
1870     priority_queue->Add(pair_entry);
1871     return;
1872   }
1873 
1874   PairEntry* const pair_entry =
1875       new PairEntry(pickup, pickup_insert_after, delivery,
1876                     delivery_insert_after, vehicle, num_allowed_vehicles);
1877   pair_entry->set_value(GetInsertionValueForPairAtPositions(
1878       pickup, pickup_insert_after, delivery, delivery_insert_after, vehicle));
1879 
1880   // Add entry to priority_queue and pickup_/delivery_entries.
1881   DCHECK(!priority_queue->Contains(pair_entry));
1882   pickup_entries->at(pickup_insert_after).insert(pair_entry);
1883   delivery_entries->at(delivery_insert_after).insert(pair_entry);
1884   priority_queue->Add(pair_entry);
1885 }
1886 
UpdatePairEntry(GlobalCheapestInsertionFilteredHeuristic::PairEntry * const pair_entry,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::PairEntry> * priority_queue) const1887 void GlobalCheapestInsertionFilteredHeuristic::UpdatePairEntry(
1888     GlobalCheapestInsertionFilteredHeuristic::PairEntry* const pair_entry,
1889     AdjustablePriorityQueue<
1890         GlobalCheapestInsertionFilteredHeuristic::PairEntry>* priority_queue)
1891     const {
1892   pair_entry->set_value(GetInsertionValueForPairAtPositions(
1893       pair_entry->pickup_to_insert(), pair_entry->pickup_insert_after(),
1894       pair_entry->delivery_to_insert(), pair_entry->delivery_insert_after(),
1895       pair_entry->vehicle()));
1896 
1897   // Update the priority_queue.
1898   DCHECK(priority_queue->Contains(pair_entry));
1899   priority_queue->NoteChangedPriority(pair_entry);
1900 }
1901 
1902 int64_t
GetInsertionValueForPairAtPositions(int64_t pickup,int64_t pickup_insert_after,int64_t delivery,int64_t delivery_insert_after,int vehicle) const1903 GlobalCheapestInsertionFilteredHeuristic::GetInsertionValueForPairAtPositions(
1904     int64_t pickup, int64_t pickup_insert_after, int64_t delivery,
1905     int64_t delivery_insert_after, int vehicle) const {
1906   DCHECK_GE(pickup_insert_after, 0);
1907   const int64_t pickup_insert_before = Value(pickup_insert_after);
1908   const int64_t pickup_value = GetInsertionCostForNodeAtPosition(
1909       pickup, pickup_insert_after, pickup_insert_before, vehicle);
1910 
1911   DCHECK_GE(delivery_insert_after, 0);
1912   const int64_t delivery_insert_before = (delivery_insert_after == pickup)
1913                                              ? pickup_insert_before
1914                                              : Value(delivery_insert_after);
1915   const int64_t delivery_value = GetInsertionCostForNodeAtPosition(
1916       delivery, delivery_insert_after, delivery_insert_before, vehicle);
1917 
1918   const int64_t penalty_shift =
1919       absl::GetFlag(FLAGS_routing_shift_insertion_cost_by_penalty)
1920           ? CapAdd(GetUnperformedValue(pickup), GetUnperformedValue(delivery))
1921           : 0;
1922   return CapSub(CapAdd(pickup_value, delivery_value), penalty_shift);
1923 }
1924 
InitializePositions(const std::vector<int> & nodes,const absl::flat_hash_set<int> & vehicles,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::NodeEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::NodeEntries> * position_to_node_entries)1925 bool GlobalCheapestInsertionFilteredHeuristic::InitializePositions(
1926     const std::vector<int>& nodes, const absl::flat_hash_set<int>& vehicles,
1927     AdjustablePriorityQueue<
1928         GlobalCheapestInsertionFilteredHeuristic::NodeEntry>* priority_queue,
1929     std::vector<GlobalCheapestInsertionFilteredHeuristic::NodeEntries>*
1930         position_to_node_entries) {
1931   priority_queue->Clear();
1932   position_to_node_entries->clear();
1933   position_to_node_entries->resize(model()->Size());
1934 
1935   const int num_vehicles =
1936       vehicles.empty() ? model()->vehicles() : vehicles.size();
1937   const bool all_vehicles = (num_vehicles == model()->vehicles());
1938 
1939   for (int node : nodes) {
1940     if (Contains(node)) {
1941       continue;
1942     }
1943     if (StopSearchAndCleanup(priority_queue)) return false;
1944     // Add insertion entry making node unperformed.
1945     if (gci_params_.add_unperformed_entries &&
1946         GetUnperformedValue(node) != std::numeric_limits<int64_t>::max()) {
1947       AddNodeEntry(node, -1, -1, all_vehicles, priority_queue, nullptr);
1948     }
1949     // Add all insertion entries making node performed.
1950     InitializeInsertionEntriesPerformingNode(node, vehicles, priority_queue,
1951                                              position_to_node_entries);
1952   }
1953   return true;
1954 }
1955 
1956 void GlobalCheapestInsertionFilteredHeuristic::
InitializeInsertionEntriesPerformingNode(int64_t node,const absl::flat_hash_set<int> & vehicles,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::NodeEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::NodeEntries> * position_to_node_entries)1957     InitializeInsertionEntriesPerformingNode(
1958         int64_t node, const absl::flat_hash_set<int>& vehicles,
1959         AdjustablePriorityQueue<
1960             GlobalCheapestInsertionFilteredHeuristic::NodeEntry>*
1961             priority_queue,
1962         std::vector<GlobalCheapestInsertionFilteredHeuristic::NodeEntries>*
1963             position_to_node_entries) {
1964   const int num_vehicles =
1965       vehicles.empty() ? model()->vehicles() : vehicles.size();
1966   const bool all_vehicles = (num_vehicles == model()->vehicles());
1967 
1968   if (!gci_params_.use_neighbors_ratio_for_initialization) {
1969     auto vehicles_it = vehicles.begin();
1970     for (int v = 0; v < num_vehicles; v++) {
1971       const int vehicle = vehicles.empty() ? v : *vehicles_it++;
1972 
1973       const int64_t start = model()->Start(vehicle);
1974       if (all_vehicles && VehicleIsEmpty(vehicle) &&
1975           empty_vehicle_type_curator_->GetLowestFixedCostVehicleOfType(
1976               empty_vehicle_type_curator_->Type(vehicle)) != vehicle) {
1977         // We only consider the least expensive empty vehicle of each type for
1978         // entries.
1979         continue;
1980       }
1981       std::vector<NodeInsertion> insertions;
1982       AppendInsertionPositionsAfter(node, start, Value(start), vehicle,
1983                                     &insertions);
1984       for (const auto [insert_after, insertion_vehicle, value] : insertions) {
1985         DCHECK_EQ(insertion_vehicle, vehicle);
1986         AddNodeEntry(node, insert_after, vehicle, all_vehicles, priority_queue,
1987                      position_to_node_entries);
1988       }
1989     }
1990     return;
1991   }
1992 
1993   // We're only considering the closest neighbors as insertion positions for
1994   // the node.
1995   const auto insert_on_vehicle_for_cost_class = [this, &vehicles, all_vehicles](
1996                                                     int v, int cost_class) {
1997     return (model()->GetCostClassIndexOfVehicle(v).value() == cost_class) &&
1998            (all_vehicles || vehicles.contains(v));
1999   };
2000   for (int cost_class = 0; cost_class < model()->GetCostClassesCount();
2001        cost_class++) {
2002     for (const int64_t insert_after :
2003          GetNeighborsOfNodeForCostClass(cost_class, node)) {
2004       if (!Contains(insert_after)) {
2005         continue;
2006       }
2007       const int vehicle = node_index_to_vehicle_[insert_after];
2008       if (vehicle == -1 ||
2009           !insert_on_vehicle_for_cost_class(vehicle, cost_class)) {
2010         continue;
2011       }
2012       if (all_vehicles && VehicleIsEmpty(vehicle) &&
2013           empty_vehicle_type_curator_->GetLowestFixedCostVehicleOfType(
2014               empty_vehicle_type_curator_->Type(vehicle)) != vehicle) {
2015         // We only consider the least expensive empty vehicle of each type for
2016         // entries.
2017         continue;
2018       }
2019       AddNodeEntry(node, insert_after, vehicle, all_vehicles, priority_queue,
2020                    position_to_node_entries);
2021     }
2022   }
2023 }
2024 
UpdatePositions(const std::vector<int> & nodes,int vehicle,int64_t insert_after,bool all_vehicles,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::NodeEntry> * priority_queue,std::vector<GlobalCheapestInsertionFilteredHeuristic::NodeEntries> * node_entries)2025 bool GlobalCheapestInsertionFilteredHeuristic::UpdatePositions(
2026     const std::vector<int>& nodes, int vehicle, int64_t insert_after,
2027     bool all_vehicles,
2028     AdjustablePriorityQueue<
2029         GlobalCheapestInsertionFilteredHeuristic::NodeEntry>* priority_queue,
2030     std::vector<GlobalCheapestInsertionFilteredHeuristic::NodeEntries>*
2031         node_entries) {
2032   // Either create new entries if we are inserting after a newly inserted node
2033   // or remove entries which have already been inserted.
2034   std::vector<NodeEntry*> to_remove;
2035   absl::flat_hash_set<int> existing_insertions;
2036   for (NodeEntry* const node_entry : node_entries->at(insert_after)) {
2037     DCHECK_EQ(node_entry->insert_after(), insert_after);
2038     const int64_t node_to_insert = node_entry->node_to_insert();
2039     if (Contains(node_to_insert)) {
2040       to_remove.push_back(node_entry);
2041     } else {
2042       UpdateNodeEntry(node_entry, priority_queue);
2043       existing_insertions.insert(node_to_insert);
2044     }
2045   }
2046   for (NodeEntry* const node_entry : to_remove) {
2047     DeleteNodeEntry(node_entry, priority_queue, node_entries);
2048   }
2049   const int cost_class = model()->GetCostClassIndexOfVehicle(vehicle).value();
2050   for (int node_to_insert : nodes) {
2051     if (StopSearchAndCleanup(priority_queue)) return false;
2052     if (!Contains(node_to_insert) &&
2053         !existing_insertions.contains(node_to_insert) &&
2054         IsNeighborForCostClass(cost_class, insert_after, node_to_insert)) {
2055       AddNodeEntry(node_to_insert, insert_after, vehicle, all_vehicles,
2056                    priority_queue, node_entries);
2057     }
2058   }
2059   return true;
2060 }
2061 
DeleteNodeEntry(GlobalCheapestInsertionFilteredHeuristic::NodeEntry * entry,AdjustablePriorityQueue<GlobalCheapestInsertionFilteredHeuristic::NodeEntry> * priority_queue,std::vector<NodeEntries> * node_entries)2062 void GlobalCheapestInsertionFilteredHeuristic::DeleteNodeEntry(
2063     GlobalCheapestInsertionFilteredHeuristic::NodeEntry* entry,
2064     AdjustablePriorityQueue<
2065         GlobalCheapestInsertionFilteredHeuristic::NodeEntry>* priority_queue,
2066     std::vector<NodeEntries>* node_entries) {
2067   priority_queue->Remove(entry);
2068   if (entry->insert_after() != -1) {
2069     node_entries->at(entry->insert_after()).erase(entry);
2070   }
2071   delete entry;
2072 }
2073 
AddNodeEntry(int64_t node,int64_t insert_after,int vehicle,bool all_vehicles,AdjustablePriorityQueue<NodeEntry> * priority_queue,std::vector<NodeEntries> * node_entries) const2074 void GlobalCheapestInsertionFilteredHeuristic::AddNodeEntry(
2075     int64_t node, int64_t insert_after, int vehicle, bool all_vehicles,
2076     AdjustablePriorityQueue<NodeEntry>* priority_queue,
2077     std::vector<NodeEntries>* node_entries) const {
2078   const int64_t node_penalty = GetUnperformedValue(node);
2079   const int64_t penalty_shift =
2080       absl::GetFlag(FLAGS_routing_shift_insertion_cost_by_penalty)
2081           ? node_penalty
2082           : 0;
2083   const IntVar* const vehicle_var = model()->VehicleVar(node);
2084   if (!vehicle_var->Contains(vehicle)) {
2085     if (vehicle == -1 || !VehicleIsEmpty(vehicle)) return;
2086     // We need to check there is not an equivalent empty vehicle the node
2087     // could fit on.
2088     const auto vehicle_is_compatible = [vehicle_var](int vehicle) {
2089       return vehicle_var->Contains(vehicle);
2090     };
2091     if (!empty_vehicle_type_curator_->HasCompatibleVehicleOfType(
2092             empty_vehicle_type_curator_->Type(vehicle),
2093             vehicle_is_compatible)) {
2094       return;
2095     }
2096   }
2097   const int num_allowed_vehicles = vehicle_var->Size();
2098   if (insert_after == -1) {
2099     DCHECK_EQ(vehicle, -1);
2100     if (!all_vehicles) {
2101       // NOTE: In the case where we're not considering all routes
2102       // simultaneously, we don't add insertion entries making nodes
2103       // unperformed.
2104       return;
2105     }
2106     NodeEntry* const node_entry =
2107         new NodeEntry(node, -1, -1, num_allowed_vehicles);
2108     node_entry->set_value(CapSub(node_penalty, penalty_shift));
2109     priority_queue->Add(node_entry);
2110     return;
2111   }
2112 
2113   const int64_t insertion_cost = GetInsertionCostForNodeAtPosition(
2114       node, insert_after, Value(insert_after), vehicle);
2115   if (!all_vehicles && insertion_cost > node_penalty) {
2116     // NOTE: When all vehicles aren't considered for insertion, we don't
2117     // add entries making nodes unperformed, so we don't add insertions
2118     // which cost more than the node penalty either.
2119     return;
2120   }
2121 
2122   NodeEntry* const node_entry =
2123       new NodeEntry(node, insert_after, vehicle, num_allowed_vehicles);
2124   node_entry->set_value(CapSub(insertion_cost, penalty_shift));
2125   // Add entry to priority_queue and node_entries.
2126   DCHECK(!priority_queue->Contains(node_entry));
2127   node_entries->at(insert_after).insert(node_entry);
2128   priority_queue->Add(node_entry);
2129 }
2130 
UpdateNodeEntry(NodeEntry * const node_entry,AdjustablePriorityQueue<NodeEntry> * priority_queue) const2131 void GlobalCheapestInsertionFilteredHeuristic::UpdateNodeEntry(
2132     NodeEntry* const node_entry,
2133     AdjustablePriorityQueue<NodeEntry>* priority_queue) const {
2134   const int64_t node = node_entry->node_to_insert();
2135   const int64_t insert_after = node_entry->insert_after();
2136   DCHECK_GE(insert_after, 0);
2137   const int64_t insertion_cost = GetInsertionCostForNodeAtPosition(
2138       node, insert_after, Value(insert_after), node_entry->vehicle());
2139   const int64_t penalty_shift =
2140       absl::GetFlag(FLAGS_routing_shift_insertion_cost_by_penalty)
2141           ? GetUnperformedValue(node)
2142           : 0;
2143 
2144   node_entry->set_value(CapSub(insertion_cost, penalty_shift));
2145   // Update the priority_queue.
2146   DCHECK(priority_queue->Contains(node_entry));
2147   priority_queue->NoteChangedPriority(node_entry);
2148 }
2149 
2150 // LocalCheapestInsertionFilteredHeuristic
2151 // TODO(user): Add support for penalty costs.
2152 LocalCheapestInsertionFilteredHeuristic::
LocalCheapestInsertionFilteredHeuristic(RoutingModel * model,std::function<int64_t (int64_t,int64_t,int64_t)> evaluator,LocalSearchFilterManager * filter_manager)2153     LocalCheapestInsertionFilteredHeuristic(
2154         RoutingModel* model,
2155         std::function<int64_t(int64_t, int64_t, int64_t)> evaluator,
2156         LocalSearchFilterManager* filter_manager)
2157     : CheapestInsertionFilteredHeuristic(model, std::move(evaluator), nullptr,
2158                                          filter_manager) {
2159   std::vector<int> all_vehicles(model->vehicles());
2160   std::iota(std::begin(all_vehicles), std::end(all_vehicles), 0);
2161 
2162   start_end_distances_per_node_ =
2163       ComputeStartEndDistanceForVehicles(all_vehicles);
2164 }
2165 
BuildSolutionInternal()2166 bool LocalCheapestInsertionFilteredHeuristic::BuildSolutionInternal() {
2167   // Marking if we've tried inserting a node.
2168   std::vector<bool> visited(model()->Size(), false);
2169   // Possible positions where the current pickup can be inserted.
2170   std::vector<NodeInsertion> pickup_insertion_positions;
2171   // Possible positions where its associated delivery node can be inserted (if
2172   // the current node has one).
2173   std::vector<NodeInsertion> delivery_insertion_positions;
2174   // Iterating on pickup and delivery pairs
2175   const RoutingModel::IndexPairs& index_pairs =
2176       model()->GetPickupAndDeliveryPairs();
2177   // Sort pairs according to number of possible vehicles.
2178   std::vector<std::pair<int, int>> domain_to_pair;
2179   for (int i = 0; i < index_pairs.size(); ++i) {
2180     uint64_t domain = kint64max;
2181     for (int64_t pickup : index_pairs[i].first) {
2182       domain = std::min(domain, model()->VehicleVar(pickup)->Size());
2183     }
2184     for (int64_t delivery : index_pairs[i].first) {
2185       domain = std::min(domain, model()->VehicleVar(delivery)->Size());
2186     }
2187     domain_to_pair.emplace_back(domain, i);
2188   }
2189   std::sort(domain_to_pair.begin(), domain_to_pair.end());
2190   for (const auto [domain, pair] : domain_to_pair) {
2191     const auto& index_pair = index_pairs[pair];
2192     for (int64_t pickup : index_pair.first) {
2193       if (Contains(pickup)) {
2194         continue;
2195       }
2196       for (int64_t delivery : index_pair.second) {
2197         // If either is already in the solution, let it be inserted in the
2198         // standard node insertion loop.
2199         if (Contains(delivery)) {
2200           continue;
2201         }
2202         if (StopSearch()) return false;
2203         visited[pickup] = true;
2204         visited[delivery] = true;
2205         ComputeEvaluatorSortedPositions(pickup, &pickup_insertion_positions);
2206         for (const auto [insert_pickup_after, pickup_vehicle,
2207                          unused_pickup_value] : pickup_insertion_positions) {
2208           const int insert_pickup_before = Value(insert_pickup_after);
2209           ComputeEvaluatorSortedPositionsOnRouteAfter(
2210               delivery, pickup, insert_pickup_before, pickup_vehicle,
2211               &delivery_insertion_positions);
2212           bool found = false;
2213           for (const auto [insert_delivery_after, unused_delivery_vehicle,
2214                            unused_delivery_value] :
2215                delivery_insertion_positions) {
2216             InsertBetween(pickup, insert_pickup_after, insert_pickup_before);
2217             const int64_t insert_delivery_before =
2218                 (insert_delivery_after == insert_pickup_after) ? pickup
2219                 : (insert_delivery_after == pickup)
2220                     ? insert_pickup_before
2221                     : Value(insert_delivery_after);
2222             InsertBetween(delivery, insert_delivery_after,
2223                           insert_delivery_before);
2224             if (Commit()) {
2225               found = true;
2226               break;
2227             }
2228           }
2229           if (found) {
2230             break;
2231           }
2232         }
2233       }
2234     }
2235   }
2236 
2237   std::priority_queue<Seed> node_queue;
2238   InitializePriorityQueue(&start_end_distances_per_node_, &node_queue);
2239 
2240   // Possible positions where the current node can be inserted.
2241   std::vector<NodeInsertion> insertion_positions;
2242   while (!node_queue.empty()) {
2243     const int node = node_queue.top().second;
2244     node_queue.pop();
2245     if (Contains(node) || visited[node]) continue;
2246     ComputeEvaluatorSortedPositions(node, &insertion_positions);
2247     for (const auto [insert_after, unused_vehicle, unused_value] :
2248          insertion_positions) {
2249       if (StopSearch()) return false;
2250       InsertBetween(node, insert_after, Value(insert_after));
2251       if (Commit()) {
2252         break;
2253       }
2254     }
2255   }
2256   MakeUnassignedNodesUnperformed();
2257   return Commit();
2258 }
2259 
ComputeEvaluatorSortedPositions(int64_t node,std::vector<NodeInsertion> * sorted_insertions)2260 void LocalCheapestInsertionFilteredHeuristic::ComputeEvaluatorSortedPositions(
2261     int64_t node, std::vector<NodeInsertion>* sorted_insertions) {
2262   DCHECK(sorted_insertions != nullptr);
2263   DCHECK(!Contains(node));
2264   sorted_insertions->clear();
2265   const int size = model()->Size();
2266   if (node < size) {
2267     for (int vehicle = 0; vehicle < model()->vehicles(); ++vehicle) {
2268       const int64_t start = model()->Start(vehicle);
2269       AppendInsertionPositionsAfter(node, start, Value(start), vehicle,
2270                                     sorted_insertions);
2271     }
2272     std::sort(sorted_insertions->begin(), sorted_insertions->end());
2273   }
2274 }
2275 
2276 void LocalCheapestInsertionFilteredHeuristic::
ComputeEvaluatorSortedPositionsOnRouteAfter(int64_t node,int64_t start,int64_t next_after_start,int vehicle,std::vector<NodeInsertion> * sorted_insertions)2277     ComputeEvaluatorSortedPositionsOnRouteAfter(
2278         int64_t node, int64_t start, int64_t next_after_start, int vehicle,
2279         std::vector<NodeInsertion>* sorted_insertions) {
2280   DCHECK(sorted_insertions != nullptr);
2281   DCHECK(!Contains(node));
2282   sorted_insertions->clear();
2283   const int size = model()->Size();
2284   if (node < size) {
2285     AppendInsertionPositionsAfter(node, start, next_after_start, vehicle,
2286                                   sorted_insertions);
2287     std::sort(sorted_insertions->begin(), sorted_insertions->end());
2288   }
2289 }
2290 
2291 // CheapestAdditionFilteredHeuristic
2292 
CheapestAdditionFilteredHeuristic(RoutingModel * model,LocalSearchFilterManager * filter_manager)2293 CheapestAdditionFilteredHeuristic::CheapestAdditionFilteredHeuristic(
2294     RoutingModel* model, LocalSearchFilterManager* filter_manager)
2295     : RoutingFilteredHeuristic(model, filter_manager) {}
2296 
BuildSolutionInternal()2297 bool CheapestAdditionFilteredHeuristic::BuildSolutionInternal() {
2298   const int kUnassigned = -1;
2299   const RoutingModel::IndexPairs& pairs = model()->GetPickupAndDeliveryPairs();
2300   std::vector<std::vector<int64_t>> deliveries(Size());
2301   std::vector<std::vector<int64_t>> pickups(Size());
2302   for (const RoutingModel::IndexPair& pair : pairs) {
2303     for (int first : pair.first) {
2304       for (int second : pair.second) {
2305         deliveries[first].push_back(second);
2306         pickups[second].push_back(first);
2307       }
2308     }
2309   }
2310   // To mimic the behavior of PathSelector (cf. search.cc), iterating on
2311   // routes with partial route at their start first then on routes with largest
2312   // index.
2313   std::vector<int> sorted_vehicles(model()->vehicles(), 0);
2314   for (int vehicle = 0; vehicle < model()->vehicles(); ++vehicle) {
2315     sorted_vehicles[vehicle] = vehicle;
2316   }
2317   std::sort(sorted_vehicles.begin(), sorted_vehicles.end(),
2318             PartialRoutesAndLargeVehicleIndicesFirst(*this));
2319   // Neighbors of the node currently being extended.
2320   for (const int vehicle : sorted_vehicles) {
2321     int64_t last_node = GetStartChainEnd(vehicle);
2322     bool extend_route = true;
2323     // Extend the route of the current vehicle while it's possible. We can
2324     // iterate more than once if pickup and delivery pairs have been inserted
2325     // in the last iteration (see comment below); the new iteration will try to
2326     // extend the route after the last delivery on the route.
2327     while (extend_route) {
2328       extend_route = false;
2329       bool found = true;
2330       int64_t index = last_node;
2331       int64_t end = GetEndChainStart(vehicle);
2332       // Extend the route until either the end node of the vehicle is reached
2333       // or no node or node pair can be added. Deliveries in pickup and
2334       // delivery pairs are added at the same time as pickups, at the end of the
2335       // route, in reverse order of the pickups. Deliveries are never added
2336       // alone.
2337       while (found && !model()->IsEnd(index)) {
2338         found = false;
2339         std::vector<int64_t> neighbors;
2340         if (index < model()->Nexts().size()) {
2341           std::unique_ptr<IntVarIterator> it(
2342               model()->Nexts()[index]->MakeDomainIterator(false));
2343           auto next_values = InitAndGetValues(it.get());
2344           neighbors = GetPossibleNextsFromIterator(index, next_values.begin(),
2345                                                    next_values.end());
2346         }
2347         for (int i = 0; !found && i < neighbors.size(); ++i) {
2348           int64_t next = -1;
2349           switch (i) {
2350             case 0:
2351               next = FindTopSuccessor(index, neighbors);
2352               break;
2353             case 1:
2354               SortSuccessors(index, &neighbors);
2355               ABSL_FALLTHROUGH_INTENDED;
2356             default:
2357               next = neighbors[i];
2358           }
2359           if (model()->IsEnd(next) && next != end) {
2360             continue;
2361           }
2362           // Only add a delivery if one of its pickups has been added already.
2363           if (!model()->IsEnd(next) && !pickups[next].empty()) {
2364             bool contains_pickups = false;
2365             for (int64_t pickup : pickups[next]) {
2366               if (Contains(pickup)) {
2367                 contains_pickups = true;
2368                 break;
2369               }
2370             }
2371             if (!contains_pickups) {
2372               continue;
2373             }
2374           }
2375           std::vector<int64_t> next_deliveries;
2376           if (next < deliveries.size()) {
2377             next_deliveries = GetPossibleNextsFromIterator(
2378                 next, deliveries[next].begin(), deliveries[next].end());
2379           }
2380           if (next_deliveries.empty()) next_deliveries = {kUnassigned};
2381           for (int j = 0; !found && j < next_deliveries.size(); ++j) {
2382             if (StopSearch()) return false;
2383             int delivery = -1;
2384             switch (j) {
2385               case 0:
2386                 delivery = FindTopSuccessor(next, next_deliveries);
2387                 break;
2388               case 1:
2389                 SortSuccessors(next, &next_deliveries);
2390                 ABSL_FALLTHROUGH_INTENDED;
2391               default:
2392                 delivery = next_deliveries[j];
2393             }
2394             // Insert "next" after "index", and before "end" if it is not the
2395             // end already.
2396             SetValue(index, next);
2397             if (!model()->IsEnd(next)) {
2398               SetValue(next, end);
2399               MakeDisjunctionNodesUnperformed(next);
2400               if (delivery != kUnassigned) {
2401                 SetValue(next, delivery);
2402                 SetValue(delivery, end);
2403                 MakeDisjunctionNodesUnperformed(delivery);
2404               }
2405             }
2406             if (Commit()) {
2407               index = next;
2408               found = true;
2409               if (delivery != kUnassigned) {
2410                 if (model()->IsEnd(end) && last_node != delivery) {
2411                   last_node = delivery;
2412                   extend_route = true;
2413                 }
2414                 end = delivery;
2415               }
2416               break;
2417             }
2418           }
2419         }
2420       }
2421     }
2422   }
2423   MakeUnassignedNodesUnperformed();
2424   return Commit();
2425 }
2426 
2427 bool CheapestAdditionFilteredHeuristic::
operator ()(int vehicle1,int vehicle2) const2428     PartialRoutesAndLargeVehicleIndicesFirst::operator()(int vehicle1,
2429                                                          int vehicle2) const {
2430   const bool has_partial_route1 = (builder_.model()->Start(vehicle1) !=
2431                                    builder_.GetStartChainEnd(vehicle1));
2432   const bool has_partial_route2 = (builder_.model()->Start(vehicle2) !=
2433                                    builder_.GetStartChainEnd(vehicle2));
2434   if (has_partial_route1 == has_partial_route2) {
2435     return vehicle2 < vehicle1;
2436   } else {
2437     return has_partial_route2 < has_partial_route1;
2438   }
2439 }
2440 
2441 // EvaluatorCheapestAdditionFilteredHeuristic
2442 
2443 EvaluatorCheapestAdditionFilteredHeuristic::
EvaluatorCheapestAdditionFilteredHeuristic(RoutingModel * model,std::function<int64_t (int64_t,int64_t)> evaluator,LocalSearchFilterManager * filter_manager)2444     EvaluatorCheapestAdditionFilteredHeuristic(
2445         RoutingModel* model, std::function<int64_t(int64_t, int64_t)> evaluator,
2446         LocalSearchFilterManager* filter_manager)
2447     : CheapestAdditionFilteredHeuristic(model, filter_manager),
2448       evaluator_(std::move(evaluator)) {}
2449 
FindTopSuccessor(int64_t node,const std::vector<int64_t> & successors)2450 int64_t EvaluatorCheapestAdditionFilteredHeuristic::FindTopSuccessor(
2451     int64_t node, const std::vector<int64_t>& successors) {
2452   int64_t best_evaluation = std::numeric_limits<int64_t>::max();
2453   int64_t best_successor = -1;
2454   for (int64_t successor : successors) {
2455     const int64_t evaluation = (successor >= 0)
2456                                    ? evaluator_(node, successor)
2457                                    : std::numeric_limits<int64_t>::max();
2458     if (evaluation < best_evaluation ||
2459         (evaluation == best_evaluation && successor > best_successor)) {
2460       best_evaluation = evaluation;
2461       best_successor = successor;
2462     }
2463   }
2464   return best_successor;
2465 }
2466 
SortSuccessors(int64_t node,std::vector<int64_t> * successors)2467 void EvaluatorCheapestAdditionFilteredHeuristic::SortSuccessors(
2468     int64_t node, std::vector<int64_t>* successors) {
2469   std::vector<std::pair<int64_t, int64_t>> values;
2470   values.reserve(successors->size());
2471   for (int64_t successor : *successors) {
2472     // Tie-breaking on largest node index to mimic the behavior of
2473     // CheapestValueSelector (search.cc).
2474     values.push_back({evaluator_(node, successor), -successor});
2475   }
2476   std::sort(values.begin(), values.end());
2477   successors->clear();
2478   for (auto value : values) {
2479     successors->push_back(-value.second);
2480   }
2481 }
2482 
2483 // ComparatorCheapestAdditionFilteredHeuristic
2484 
2485 ComparatorCheapestAdditionFilteredHeuristic::
ComparatorCheapestAdditionFilteredHeuristic(RoutingModel * model,Solver::VariableValueComparator comparator,LocalSearchFilterManager * filter_manager)2486     ComparatorCheapestAdditionFilteredHeuristic(
2487         RoutingModel* model, Solver::VariableValueComparator comparator,
2488         LocalSearchFilterManager* filter_manager)
2489     : CheapestAdditionFilteredHeuristic(model, filter_manager),
2490       comparator_(std::move(comparator)) {}
2491 
FindTopSuccessor(int64_t node,const std::vector<int64_t> & successors)2492 int64_t ComparatorCheapestAdditionFilteredHeuristic::FindTopSuccessor(
2493     int64_t node, const std::vector<int64_t>& successors) {
2494   return *std::min_element(successors.begin(), successors.end(),
2495                            [this, node](int successor1, int successor2) {
2496                              return comparator_(node, successor1, successor2);
2497                            });
2498 }
2499 
SortSuccessors(int64_t node,std::vector<int64_t> * successors)2500 void ComparatorCheapestAdditionFilteredHeuristic::SortSuccessors(
2501     int64_t node, std::vector<int64_t>* successors) {
2502   std::sort(successors->begin(), successors->end(),
2503             [this, node](int successor1, int successor2) {
2504               return comparator_(node, successor1, successor2);
2505             });
2506 }
2507 
2508 // Class storing and allowing access to the savings according to the number of
2509 // vehicle types.
2510 // The savings are stored and sorted in sorted_savings_per_vehicle_type_.
2511 // Furthermore, when there is more than one vehicle type, the savings for a same
2512 // before-->after arc are sorted in costs_and_savings_per_arc_[arc] by
2513 // increasing cost(s-->before-->after-->e), where s and e are the start and end
2514 // of the route, in order to make sure the arc is served by the route with the
2515 // closest depot (start/end) possible.
2516 // When there is only one vehicle "type" (i.e. all vehicles have the same
2517 // start/end and cost class), each arc has a single saving value associated to
2518 // it, so we ignore this last step to avoid unnecessary computations, and only
2519 // work with sorted_savings_per_vehicle_type_[0].
2520 // In case of multiple vehicle types, the best savings for each arc, i.e. the
2521 // savings corresponding to the closest vehicle type, are inserted and sorted in
2522 // sorted_savings_.
2523 //
2524 // This class also handles skipped Savings:
2525 // The vectors skipped_savings_starting/ending_at_ contain all the Savings that
2526 // weren't added to the model, which we want to consider for later:
2527 // 1) When a Saving before-->after with both nodes uncontained cannot be used to
2528 //    start a new route (no more available vehicles or could not commit on any
2529 //    of those available).
2530 // 2) When only one of the nodes of the Saving is contained but on a different
2531 //    vehicle type.
2532 // In these cases, the Update() method is called with update_best_saving = true,
2533 // which in turn calls SkipSavingForArc() (within
2534 // UpdateNextAndSkippedSavingsForArcWithType()) to mark the Saving for this arc
2535 // (with the correct type in the second case) as "skipped", by storing it in
2536 // skipped_savings_starting_at_[before] and skipped_savings_ending_at_[after].
2537 //
2538 // UpdateNextAndSkippedSavingsForArcWithType() also updates the next_savings_
2539 // vector, which stores the savings to go through once we've iterated through
2540 // all sorted_savings_.
2541 // In the first case above, where neither nodes are contained, we skip the
2542 // current Saving (current_saving_), and add the next best Saving for this arc
2543 // to next_savings_ (in case this skipped Saving is never considered).
2544 // In the second case with a specific type, we search for the Saving with the
2545 // correct type for this arc, and add it to both next_savings_ and the skipped
2546 // Savings.
2547 //
2548 // The skipped Savings are then re-considered when one of their ends gets
2549 // inserted:
2550 // When another Saving other_node-->before (or after-->other_node) gets
2551 // inserted, all skipped Savings in skipped_savings_starting_at_[before] (or
2552 // skipped_savings_ending_at_[after]) are once again considered by calling
2553 // ReinjectSkippedSavingsStartingAt() (or ReinjectSkippedSavingsEndingAt()).
2554 // Then, when calling GetSaving(), we iterate through the reinjected Savings in
2555 // order of insertion in the vectors while there are reinjected savings.
2556 template <typename Saving>
2557 class SavingsFilteredHeuristic::SavingsContainer {
2558  public:
SavingsContainer(const SavingsFilteredHeuristic * savings_db,int vehicle_types)2559   explicit SavingsContainer(const SavingsFilteredHeuristic* savings_db,
2560                             int vehicle_types)
2561       : savings_db_(savings_db),
2562         index_in_sorted_savings_(0),
2563         vehicle_types_(vehicle_types),
2564         single_vehicle_type_(vehicle_types == 1),
2565         using_incoming_reinjected_saving_(false),
2566         sorted_(false),
2567         to_update_(true) {}
2568 
InitializeContainer(int64_t size,int64_t saving_neighbors)2569   void InitializeContainer(int64_t size, int64_t saving_neighbors) {
2570     sorted_savings_per_vehicle_type_.clear();
2571     sorted_savings_per_vehicle_type_.resize(vehicle_types_);
2572     for (std::vector<Saving>& savings : sorted_savings_per_vehicle_type_) {
2573       savings.reserve(size * saving_neighbors);
2574     }
2575 
2576     sorted_savings_.clear();
2577     costs_and_savings_per_arc_.clear();
2578     arc_indices_per_before_node_.clear();
2579 
2580     if (!single_vehicle_type_) {
2581       costs_and_savings_per_arc_.reserve(size * saving_neighbors);
2582       arc_indices_per_before_node_.resize(size);
2583       for (int before_node = 0; before_node < size; before_node++) {
2584         arc_indices_per_before_node_[before_node].reserve(saving_neighbors);
2585       }
2586     }
2587     skipped_savings_starting_at_.clear();
2588     skipped_savings_starting_at_.resize(size);
2589     skipped_savings_ending_at_.clear();
2590     skipped_savings_ending_at_.resize(size);
2591     incoming_reinjected_savings_ = nullptr;
2592     outgoing_reinjected_savings_ = nullptr;
2593     incoming_new_reinjected_savings_ = nullptr;
2594     outgoing_new_reinjected_savings_ = nullptr;
2595   }
2596 
AddNewSaving(const Saving & saving,int64_t total_cost,int64_t before_node,int64_t after_node,int vehicle_type)2597   void AddNewSaving(const Saving& saving, int64_t total_cost,
2598                     int64_t before_node, int64_t after_node, int vehicle_type) {
2599     CHECK(!sorted_savings_per_vehicle_type_.empty())
2600         << "Container not initialized!";
2601     sorted_savings_per_vehicle_type_[vehicle_type].push_back(saving);
2602     UpdateArcIndicesCostsAndSavings(before_node, after_node,
2603                                     {total_cost, saving});
2604   }
2605 
Sort()2606   void Sort() {
2607     CHECK(!sorted_) << "Container already sorted!";
2608 
2609     for (std::vector<Saving>& savings : sorted_savings_per_vehicle_type_) {
2610       std::sort(savings.begin(), savings.end());
2611     }
2612 
2613     if (single_vehicle_type_) {
2614       const auto& savings = sorted_savings_per_vehicle_type_[0];
2615       sorted_savings_.resize(savings.size());
2616       std::transform(savings.begin(), savings.end(), sorted_savings_.begin(),
2617                      [](const Saving& saving) {
2618                        return SavingAndArc({saving, /*arc_index*/ -1});
2619                      });
2620     } else {
2621       // For each arc, sort the savings by decreasing total cost
2622       // start-->a-->b-->end.
2623       // The best saving for each arc is therefore the last of its vector.
2624       sorted_savings_.reserve(vehicle_types_ *
2625                               costs_and_savings_per_arc_.size());
2626 
2627       for (int arc_index = 0; arc_index < costs_and_savings_per_arc_.size();
2628            arc_index++) {
2629         std::vector<std::pair<int64_t, Saving>>& costs_and_savings =
2630             costs_and_savings_per_arc_[arc_index];
2631         DCHECK(!costs_and_savings.empty());
2632 
2633         std::sort(
2634             costs_and_savings.begin(), costs_and_savings.end(),
2635             [](const std::pair<int64_t, Saving>& cs1,
2636                const std::pair<int64_t, Saving>& cs2) { return cs1 > cs2; });
2637 
2638         // Insert all Savings for this arc with the lowest cost into
2639         // sorted_savings_.
2640         // TODO(user): Also do this when reiterating on next_savings_.
2641         const int64_t cost = costs_and_savings.back().first;
2642         while (!costs_and_savings.empty() &&
2643                costs_and_savings.back().first == cost) {
2644           sorted_savings_.push_back(
2645               {costs_and_savings.back().second, arc_index});
2646           costs_and_savings.pop_back();
2647         }
2648       }
2649       std::sort(sorted_savings_.begin(), sorted_savings_.end());
2650       next_saving_type_and_index_for_arc_.clear();
2651       next_saving_type_and_index_for_arc_.resize(
2652           costs_and_savings_per_arc_.size(), {-1, -1});
2653     }
2654     sorted_ = true;
2655     index_in_sorted_savings_ = 0;
2656     to_update_ = false;
2657   }
2658 
HasSaving()2659   bool HasSaving() {
2660     return index_in_sorted_savings_ < sorted_savings_.size() ||
2661            HasReinjectedSavings();
2662   }
2663 
GetSaving()2664   Saving GetSaving() {
2665     CHECK(sorted_) << "Calling GetSaving() before Sort() !";
2666     CHECK(!to_update_)
2667         << "Update() should be called between two calls to GetSaving() !";
2668 
2669     to_update_ = true;
2670 
2671     if (HasReinjectedSavings()) {
2672       if (incoming_reinjected_savings_ != nullptr &&
2673           outgoing_reinjected_savings_ != nullptr) {
2674         // Get the best Saving among the two.
2675         SavingAndArc& incoming_saving = incoming_reinjected_savings_->front();
2676         SavingAndArc& outgoing_saving = outgoing_reinjected_savings_->front();
2677         if (incoming_saving < outgoing_saving) {
2678           current_saving_ = incoming_saving;
2679           using_incoming_reinjected_saving_ = true;
2680         } else {
2681           current_saving_ = outgoing_saving;
2682           using_incoming_reinjected_saving_ = false;
2683         }
2684       } else {
2685         if (incoming_reinjected_savings_ != nullptr) {
2686           current_saving_ = incoming_reinjected_savings_->front();
2687           using_incoming_reinjected_saving_ = true;
2688         }
2689         if (outgoing_reinjected_savings_ != nullptr) {
2690           current_saving_ = outgoing_reinjected_savings_->front();
2691           using_incoming_reinjected_saving_ = false;
2692         }
2693       }
2694     } else {
2695       current_saving_ = sorted_savings_[index_in_sorted_savings_];
2696     }
2697     return current_saving_.saving;
2698   }
2699 
Update(bool update_best_saving,int type=-1)2700   void Update(bool update_best_saving, int type = -1) {
2701     CHECK(to_update_) << "Container already up to date!";
2702     if (update_best_saving) {
2703       const int64_t arc_index = current_saving_.arc_index;
2704       UpdateNextAndSkippedSavingsForArcWithType(arc_index, type);
2705     }
2706     if (!HasReinjectedSavings()) {
2707       index_in_sorted_savings_++;
2708 
2709       if (index_in_sorted_savings_ == sorted_savings_.size()) {
2710         sorted_savings_.swap(next_savings_);
2711         gtl::STLClearObject(&next_savings_);
2712         index_in_sorted_savings_ = 0;
2713 
2714         std::sort(sorted_savings_.begin(), sorted_savings_.end());
2715         next_saving_type_and_index_for_arc_.clear();
2716         next_saving_type_and_index_for_arc_.resize(
2717             costs_and_savings_per_arc_.size(), {-1, -1});
2718       }
2719     }
2720     UpdateReinjectedSavings();
2721     to_update_ = false;
2722   }
2723 
UpdateWithType(int type)2724   void UpdateWithType(int type) {
2725     CHECK(!single_vehicle_type_);
2726     Update(/*update_best_saving*/ true, type);
2727   }
2728 
GetSortedSavingsForVehicleType(int type)2729   const std::vector<Saving>& GetSortedSavingsForVehicleType(int type) {
2730     CHECK(sorted_) << "Savings not sorted yet!";
2731     CHECK_LT(type, vehicle_types_);
2732     return sorted_savings_per_vehicle_type_[type];
2733   }
2734 
ReinjectSkippedSavingsStartingAt(int64_t node)2735   void ReinjectSkippedSavingsStartingAt(int64_t node) {
2736     CHECK(outgoing_new_reinjected_savings_ == nullptr);
2737     outgoing_new_reinjected_savings_ = &(skipped_savings_starting_at_[node]);
2738   }
2739 
ReinjectSkippedSavingsEndingAt(int64_t node)2740   void ReinjectSkippedSavingsEndingAt(int64_t node) {
2741     CHECK(incoming_new_reinjected_savings_ == nullptr);
2742     incoming_new_reinjected_savings_ = &(skipped_savings_ending_at_[node]);
2743   }
2744 
2745  private:
2746   struct SavingAndArc {
2747     Saving saving;
2748     int64_t arc_index;
2749 
operator <operations_research::SavingsFilteredHeuristic::SavingsContainer::SavingAndArc2750     bool operator<(const SavingAndArc& other) const {
2751       return std::tie(saving, arc_index) <
2752              std::tie(other.saving, other.arc_index);
2753     }
2754   };
2755 
2756   // Skips the Saving for the arc before_node-->after_node, by adding it to the
2757   // skipped_savings_ vector of the nodes, if they're uncontained.
SkipSavingForArc(const SavingAndArc & saving_and_arc)2758   void SkipSavingForArc(const SavingAndArc& saving_and_arc) {
2759     const Saving& saving = saving_and_arc.saving;
2760     const int64_t before_node = savings_db_->GetBeforeNodeFromSaving(saving);
2761     const int64_t after_node = savings_db_->GetAfterNodeFromSaving(saving);
2762     if (!savings_db_->Contains(before_node)) {
2763       skipped_savings_starting_at_[before_node].push_back(saving_and_arc);
2764     }
2765     if (!savings_db_->Contains(after_node)) {
2766       skipped_savings_ending_at_[after_node].push_back(saving_and_arc);
2767     }
2768   }
2769 
2770   // Called within Update() when update_best_saving is true, this method updates
2771   // the next_savings_ and skipped savings vectors for a given arc_index and
2772   // vehicle type.
2773   // When a Saving with the right type has already been added to next_savings_
2774   // for this arc, no action is needed on next_savings_.
2775   // Otherwise, if such a Saving exists, GetNextSavingForArcWithType() will find
2776   // and assign it to next_saving, which is then used to update next_savings_.
2777   // Finally, the right Saving is skipped for this arc: if looking for a
2778   // specific type (i.e. type != -1), next_saving (which has the correct type)
2779   // is skipped, otherwise the current_saving_ is.
UpdateNextAndSkippedSavingsForArcWithType(int64_t arc_index,int type)2780   void UpdateNextAndSkippedSavingsForArcWithType(int64_t arc_index, int type) {
2781     if (single_vehicle_type_) {
2782       // No next Saving, skip the current Saving.
2783       CHECK_EQ(type, -1);
2784       SkipSavingForArc(current_saving_);
2785       return;
2786     }
2787     CHECK_GE(arc_index, 0);
2788     auto& type_and_index = next_saving_type_and_index_for_arc_[arc_index];
2789     const int previous_index = type_and_index.second;
2790     const int previous_type = type_and_index.first;
2791     bool next_saving_added = false;
2792     Saving next_saving;
2793 
2794     if (previous_index >= 0) {
2795       // Next Saving already added for this arc.
2796       DCHECK_GE(previous_type, 0);
2797       if (type == -1 || previous_type == type) {
2798         // Not looking for a specific type, or correct type already in
2799         // next_savings_.
2800         next_saving_added = true;
2801         next_saving = next_savings_[previous_index].saving;
2802       }
2803     }
2804 
2805     if (!next_saving_added &&
2806         GetNextSavingForArcWithType(arc_index, type, &next_saving)) {
2807       type_and_index.first = savings_db_->GetVehicleTypeFromSaving(next_saving);
2808       if (previous_index >= 0) {
2809         // Update the previous saving.
2810         next_savings_[previous_index] = {next_saving, arc_index};
2811       } else {
2812         // Insert the new next Saving for this arc.
2813         type_and_index.second = next_savings_.size();
2814         next_savings_.push_back({next_saving, arc_index});
2815       }
2816       next_saving_added = true;
2817     }
2818 
2819     // Skip the Saving based on the vehicle type.
2820     if (type == -1) {
2821       // Skip the current Saving.
2822       SkipSavingForArc(current_saving_);
2823     } else {
2824       // Skip the Saving with the correct type, already added to next_savings_
2825       // if it was found.
2826       if (next_saving_added) {
2827         SkipSavingForArc({next_saving, arc_index});
2828       }
2829     }
2830   }
2831 
UpdateReinjectedSavings()2832   void UpdateReinjectedSavings() {
2833     UpdateGivenReinjectedSavings(incoming_new_reinjected_savings_,
2834                                  &incoming_reinjected_savings_,
2835                                  using_incoming_reinjected_saving_);
2836     UpdateGivenReinjectedSavings(outgoing_new_reinjected_savings_,
2837                                  &outgoing_reinjected_savings_,
2838                                  !using_incoming_reinjected_saving_);
2839     incoming_new_reinjected_savings_ = nullptr;
2840     outgoing_new_reinjected_savings_ = nullptr;
2841   }
2842 
UpdateGivenReinjectedSavings(std::deque<SavingAndArc> * new_reinjected_savings,std::deque<SavingAndArc> ** reinjected_savings,bool using_reinjected_savings)2843   void UpdateGivenReinjectedSavings(
2844       std::deque<SavingAndArc>* new_reinjected_savings,
2845       std::deque<SavingAndArc>** reinjected_savings,
2846       bool using_reinjected_savings) {
2847     if (new_reinjected_savings == nullptr) {
2848       // No new reinjected savings, update the previous ones if needed.
2849       if (*reinjected_savings != nullptr && using_reinjected_savings) {
2850         CHECK(!(*reinjected_savings)->empty());
2851         (*reinjected_savings)->pop_front();
2852         if ((*reinjected_savings)->empty()) {
2853           *reinjected_savings = nullptr;
2854         }
2855       }
2856       return;
2857     }
2858 
2859     // New savings reinjected.
2860     // Forget about the previous reinjected savings and add the new ones if
2861     // there are any.
2862     if (*reinjected_savings != nullptr) {
2863       (*reinjected_savings)->clear();
2864     }
2865     *reinjected_savings = nullptr;
2866     if (!new_reinjected_savings->empty()) {
2867       *reinjected_savings = new_reinjected_savings;
2868     }
2869   }
2870 
HasReinjectedSavings()2871   bool HasReinjectedSavings() {
2872     return outgoing_reinjected_savings_ != nullptr ||
2873            incoming_reinjected_savings_ != nullptr;
2874   }
2875 
UpdateArcIndicesCostsAndSavings(int64_t before_node,int64_t after_node,const std::pair<int64_t,Saving> & cost_and_saving)2876   void UpdateArcIndicesCostsAndSavings(
2877       int64_t before_node, int64_t after_node,
2878       const std::pair<int64_t, Saving>& cost_and_saving) {
2879     if (single_vehicle_type_) {
2880       return;
2881     }
2882     absl::flat_hash_map<int, int>& arc_indices =
2883         arc_indices_per_before_node_[before_node];
2884     const auto& arc_inserted = arc_indices.insert(
2885         std::make_pair(after_node, costs_and_savings_per_arc_.size()));
2886     const int index = arc_inserted.first->second;
2887     if (arc_inserted.second) {
2888       costs_and_savings_per_arc_.push_back({cost_and_saving});
2889     } else {
2890       DCHECK_LT(index, costs_and_savings_per_arc_.size());
2891       costs_and_savings_per_arc_[index].push_back(cost_and_saving);
2892     }
2893   }
2894 
GetNextSavingForArcWithType(int64_t arc_index,int type,Saving * next_saving)2895   bool GetNextSavingForArcWithType(int64_t arc_index, int type,
2896                                    Saving* next_saving) {
2897     std::vector<std::pair<int64_t, Saving>>& costs_and_savings =
2898         costs_and_savings_per_arc_[arc_index];
2899 
2900     bool found_saving = false;
2901     while (!costs_and_savings.empty() && !found_saving) {
2902       const Saving& saving = costs_and_savings.back().second;
2903       if (type == -1 || savings_db_->GetVehicleTypeFromSaving(saving) == type) {
2904         *next_saving = saving;
2905         found_saving = true;
2906       }
2907       costs_and_savings.pop_back();
2908     }
2909     return found_saving;
2910   }
2911 
2912   const SavingsFilteredHeuristic* const savings_db_;
2913   int64_t index_in_sorted_savings_;
2914   std::vector<std::vector<Saving>> sorted_savings_per_vehicle_type_;
2915   std::vector<SavingAndArc> sorted_savings_;
2916   std::vector<SavingAndArc> next_savings_;
2917   std::vector<std::pair</*type*/ int, /*index*/ int>>
2918       next_saving_type_and_index_for_arc_;
2919   SavingAndArc current_saving_;
2920   std::vector<std::vector<std::pair</*cost*/ int64_t, Saving>>>
2921       costs_and_savings_per_arc_;
2922   std::vector<absl::flat_hash_map</*after_node*/ int, /*arc_index*/ int>>
2923       arc_indices_per_before_node_;
2924   std::vector<std::deque<SavingAndArc>> skipped_savings_starting_at_;
2925   std::vector<std::deque<SavingAndArc>> skipped_savings_ending_at_;
2926   std::deque<SavingAndArc>* outgoing_reinjected_savings_;
2927   std::deque<SavingAndArc>* incoming_reinjected_savings_;
2928   std::deque<SavingAndArc>* outgoing_new_reinjected_savings_;
2929   std::deque<SavingAndArc>* incoming_new_reinjected_savings_;
2930   const int vehicle_types_;
2931   const bool single_vehicle_type_;
2932   bool using_incoming_reinjected_saving_;
2933   bool sorted_;
2934   bool to_update_;
2935 };
2936 
2937 // SavingsFilteredHeuristic
2938 
SavingsFilteredHeuristic(RoutingModel * model,SavingsParameters parameters,LocalSearchFilterManager * filter_manager)2939 SavingsFilteredHeuristic::SavingsFilteredHeuristic(
2940     RoutingModel* model, SavingsParameters parameters,
2941     LocalSearchFilterManager* filter_manager)
2942     : RoutingFilteredHeuristic(model, filter_manager),
2943       vehicle_type_curator_(nullptr),
2944       savings_params_(parameters) {
2945   DCHECK_GT(savings_params_.neighbors_ratio, 0);
2946   DCHECK_LE(savings_params_.neighbors_ratio, 1);
2947   DCHECK_GT(savings_params_.max_memory_usage_bytes, 0);
2948   DCHECK_GT(savings_params_.arc_coefficient, 0);
2949   const int size = model->Size();
2950   size_squared_ = size * size;
2951 }
2952 
~SavingsFilteredHeuristic()2953 SavingsFilteredHeuristic::~SavingsFilteredHeuristic() {}
2954 
BuildSolutionInternal()2955 bool SavingsFilteredHeuristic::BuildSolutionInternal() {
2956   if (vehicle_type_curator_ == nullptr) {
2957     vehicle_type_curator_ = absl::make_unique<VehicleTypeCurator>(
2958         model()->GetVehicleTypeContainer());
2959   }
2960   // Only store empty vehicles in the vehicle_type_curator_.
2961   vehicle_type_curator_->Reset(
2962       [this](int vehicle) { return VehicleIsEmpty(vehicle); });
2963   if (!ComputeSavings()) return false;
2964   BuildRoutesFromSavings();
2965   // Free all the space used to store the Savings in the container.
2966   savings_container_.reset();
2967   MakeUnassignedNodesUnperformed();
2968   if (!Commit()) return false;
2969   MakePartiallyPerformedPairsUnperformed();
2970   return Commit();
2971 }
2972 
StartNewRouteWithBestVehicleOfType(int type,int64_t before_node,int64_t after_node)2973 int SavingsFilteredHeuristic::StartNewRouteWithBestVehicleOfType(
2974     int type, int64_t before_node, int64_t after_node) {
2975   auto vehicle_is_compatible = [this, before_node, after_node](int vehicle) {
2976     if (!model()->VehicleVar(before_node)->Contains(vehicle) ||
2977         !model()->VehicleVar(after_node)->Contains(vehicle)) {
2978       return false;
2979     }
2980     // Try to commit the arc on this vehicle.
2981     DCHECK(VehicleIsEmpty(vehicle));
2982     const int64_t start = model()->Start(vehicle);
2983     const int64_t end = model()->End(vehicle);
2984     SetValue(start, before_node);
2985     SetValue(before_node, after_node);
2986     SetValue(after_node, end);
2987     return Commit();
2988   };
2989 
2990   return vehicle_type_curator_
2991       ->GetCompatibleVehicleOfType(
2992           type, vehicle_is_compatible,
2993           /*stop_and_return_vehicle*/ [](int) { return false; })
2994       .first;
2995 }
2996 
AddSymmetricArcsToAdjacencyLists(std::vector<std::vector<int64_t>> * adjacency_lists)2997 void SavingsFilteredHeuristic::AddSymmetricArcsToAdjacencyLists(
2998     std::vector<std::vector<int64_t>>* adjacency_lists) {
2999   for (int64_t node = 0; node < adjacency_lists->size(); node++) {
3000     for (int64_t neighbor : (*adjacency_lists)[node]) {
3001       if (model()->IsStart(neighbor) || model()->IsEnd(neighbor)) {
3002         continue;
3003       }
3004       (*adjacency_lists)[neighbor].push_back(node);
3005     }
3006   }
3007   std::transform(adjacency_lists->begin(), adjacency_lists->end(),
3008                  adjacency_lists->begin(), [](std::vector<int64_t> vec) {
3009                    std::sort(vec.begin(), vec.end());
3010                    vec.erase(std::unique(vec.begin(), vec.end()), vec.end());
3011                    return vec;
3012                  });
3013 }
3014 
3015 // Computes the savings related to each pair of non-start and non-end nodes.
3016 // The savings value for an arc a-->b for a vehicle starting at node s and
3017 // ending at node e is:
3018 // saving = cost(s-->a-->e) + cost(s-->b-->e) - cost(s-->a-->b-->e), i.e.
3019 // saving = cost(a-->e) + cost(s-->b) - cost(a-->b)
3020 // The saving value also considers a coefficient for the cost of the arc
3021 // a-->b, which results in:
3022 // saving = cost(a-->e) + cost(s-->b) - [arc_coefficient_ * cost(a-->b)]
3023 // The higher this saving value, the better the arc.
3024 // Here, the value stored for the savings is -saving, which are therefore
3025 // considered in decreasing order.
ComputeSavings()3026 bool SavingsFilteredHeuristic::ComputeSavings() {
3027   const int num_vehicle_types = vehicle_type_curator_->NumTypes();
3028   const int size = model()->Size();
3029 
3030   std::vector<int64_t> uncontained_non_start_end_nodes;
3031   uncontained_non_start_end_nodes.reserve(size);
3032   for (int node = 0; node < size; node++) {
3033     if (!model()->IsStart(node) && !model()->IsEnd(node) && !Contains(node)) {
3034       uncontained_non_start_end_nodes.push_back(node);
3035     }
3036   }
3037 
3038   const int64_t saving_neighbors =
3039       std::min(MaxNumNeighborsPerNode(num_vehicle_types),
3040                static_cast<int64_t>(uncontained_non_start_end_nodes.size()));
3041 
3042   savings_container_ =
3043       absl::make_unique<SavingsContainer<Saving>>(this, num_vehicle_types);
3044   savings_container_->InitializeContainer(size, saving_neighbors);
3045   if (StopSearch()) return false;
3046   std::vector<std::vector<int64_t>> adjacency_lists(size);
3047 
3048   for (int type = 0; type < num_vehicle_types; ++type) {
3049     const int vehicle =
3050         vehicle_type_curator_->GetLowestFixedCostVehicleOfType(type);
3051     if (vehicle < 0) {
3052       continue;
3053     }
3054 
3055     const int64_t cost_class =
3056         model()->GetCostClassIndexOfVehicle(vehicle).value();
3057     const int64_t start = model()->Start(vehicle);
3058     const int64_t end = model()->End(vehicle);
3059     const int64_t fixed_cost = model()->GetFixedCostOfVehicle(vehicle);
3060 
3061     // Compute the neighbors for each non-start/end node not already inserted in
3062     // the model.
3063     for (int before_node : uncontained_non_start_end_nodes) {
3064       std::vector<std::pair</*cost*/ int64_t, /*node*/ int64_t>>
3065           costed_after_nodes;
3066       costed_after_nodes.reserve(uncontained_non_start_end_nodes.size());
3067       if (StopSearch()) return false;
3068       for (int after_node : uncontained_non_start_end_nodes) {
3069         if (after_node != before_node) {
3070           costed_after_nodes.push_back(std::make_pair(
3071               model()->GetArcCostForClass(before_node, after_node, cost_class),
3072               after_node));
3073         }
3074       }
3075       if (saving_neighbors < costed_after_nodes.size()) {
3076         std::nth_element(costed_after_nodes.begin(),
3077                          costed_after_nodes.begin() + saving_neighbors,
3078                          costed_after_nodes.end());
3079         costed_after_nodes.resize(saving_neighbors);
3080       }
3081       adjacency_lists[before_node].resize(costed_after_nodes.size());
3082       std::transform(costed_after_nodes.begin(), costed_after_nodes.end(),
3083                      adjacency_lists[before_node].begin(),
3084                      [](std::pair<int64_t, int64_t> cost_and_node) {
3085                        return cost_and_node.second;
3086                      });
3087     }
3088     if (savings_params_.add_reverse_arcs) {
3089       AddSymmetricArcsToAdjacencyLists(&adjacency_lists);
3090     }
3091     if (StopSearch()) return false;
3092 
3093     // Build the savings for this vehicle type given the adjacency_lists.
3094     for (int before_node : uncontained_non_start_end_nodes) {
3095       const int64_t before_to_end_cost =
3096           model()->GetArcCostForClass(before_node, end, cost_class);
3097       const int64_t start_to_before_cost =
3098           CapSub(model()->GetArcCostForClass(start, before_node, cost_class),
3099                  fixed_cost);
3100       if (StopSearch()) return false;
3101       for (int64_t after_node : adjacency_lists[before_node]) {
3102         if (model()->IsStart(after_node) || model()->IsEnd(after_node) ||
3103             before_node == after_node || Contains(after_node)) {
3104           continue;
3105         }
3106         const int64_t arc_cost =
3107             model()->GetArcCostForClass(before_node, after_node, cost_class);
3108         const int64_t start_to_after_cost =
3109             CapSub(model()->GetArcCostForClass(start, after_node, cost_class),
3110                    fixed_cost);
3111         const int64_t after_to_end_cost =
3112             model()->GetArcCostForClass(after_node, end, cost_class);
3113 
3114         const double weighted_arc_cost_fp =
3115             savings_params_.arc_coefficient * arc_cost;
3116         const int64_t weighted_arc_cost =
3117             weighted_arc_cost_fp < std::numeric_limits<int64_t>::max()
3118                 ? static_cast<int64_t>(weighted_arc_cost_fp)
3119                 : std::numeric_limits<int64_t>::max();
3120         const int64_t saving_value = CapSub(
3121             CapAdd(before_to_end_cost, start_to_after_cost), weighted_arc_cost);
3122 
3123         const Saving saving =
3124             BuildSaving(-saving_value, type, before_node, after_node);
3125 
3126         const int64_t total_cost =
3127             CapAdd(CapAdd(start_to_before_cost, arc_cost), after_to_end_cost);
3128 
3129         savings_container_->AddNewSaving(saving, total_cost, before_node,
3130                                          after_node, type);
3131       }
3132     }
3133   }
3134   savings_container_->Sort();
3135   return !StopSearch();
3136 }
3137 
MaxNumNeighborsPerNode(int num_vehicle_types) const3138 int64_t SavingsFilteredHeuristic::MaxNumNeighborsPerNode(
3139     int num_vehicle_types) const {
3140   const int64_t size = model()->Size();
3141 
3142   const int64_t num_neighbors_with_ratio =
3143       std::max(1.0, size * savings_params_.neighbors_ratio);
3144 
3145   // A single Saving takes 2*8 bytes of memory.
3146   // max_memory_usage_in_savings_unit = num_savings * multiplicative_factor,
3147   // Where multiplicative_factor is the memory taken (in Savings unit) for each
3148   // computed Saving.
3149   const double max_memory_usage_in_savings_unit =
3150       savings_params_.max_memory_usage_bytes / 16;
3151 
3152   // In the SavingsContainer, for each Saving, the Savings are stored:
3153   // - Once in "sorted_savings_per_vehicle_type", and (at most) once in
3154   //   "sorted_savings_" --> factor 2
3155   // - If num_vehicle_types > 1, they're also stored by arc_index in
3156   //   "costs_and_savings_per_arc", along with their int64_t cost --> factor 1.5
3157   //
3158   // On top of that,
3159   // - In the sequential version, the Saving* are also stored by in-coming and
3160   //   outgoing node (in in/out_savings_ptr), adding another 2*8 bytes per
3161   //   Saving --> factor 1.
3162   // - In the parallel version, skipped Savings are also stored in
3163   //   skipped_savings_starting/ending_at_, resulting in a maximum added factor
3164   //   of 2 for each Saving.
3165   // These extra factors are given by ExtraSavingsMemoryMultiplicativeFactor().
3166   double multiplicative_factor = 2.0 + ExtraSavingsMemoryMultiplicativeFactor();
3167   if (num_vehicle_types > 1) {
3168     multiplicative_factor += 1.5;
3169   }
3170   const double num_savings =
3171       max_memory_usage_in_savings_unit / multiplicative_factor;
3172   const int64_t num_neighbors_with_memory_restriction =
3173       std::max(1.0, num_savings / (num_vehicle_types * size));
3174 
3175   return std::min(num_neighbors_with_ratio,
3176                   num_neighbors_with_memory_restriction);
3177 }
3178 
3179 // SequentialSavingsFilteredHeuristic
3180 
BuildRoutesFromSavings()3181 void SequentialSavingsFilteredHeuristic::BuildRoutesFromSavings() {
3182   const int vehicle_types = vehicle_type_curator_->NumTypes();
3183   DCHECK_GT(vehicle_types, 0);
3184   const int size = model()->Size();
3185   // Store savings for each incoming and outgoing node and by vehicle type. This
3186   // is necessary to quickly extend partial chains without scanning all savings.
3187   std::vector<std::vector<const Saving*>> in_savings_ptr(size * vehicle_types);
3188   std::vector<std::vector<const Saving*>> out_savings_ptr(size * vehicle_types);
3189   for (int type = 0; type < vehicle_types; type++) {
3190     const int vehicle_type_offset = type * size;
3191     const std::vector<Saving>& sorted_savings_for_type =
3192         savings_container_->GetSortedSavingsForVehicleType(type);
3193     for (const Saving& saving : sorted_savings_for_type) {
3194       DCHECK_EQ(GetVehicleTypeFromSaving(saving), type);
3195       const int before_node = GetBeforeNodeFromSaving(saving);
3196       in_savings_ptr[vehicle_type_offset + before_node].push_back(&saving);
3197       const int after_node = GetAfterNodeFromSaving(saving);
3198       out_savings_ptr[vehicle_type_offset + after_node].push_back(&saving);
3199     }
3200   }
3201 
3202   // Build routes from savings.
3203   while (savings_container_->HasSaving()) {
3204     if (StopSearch()) return;
3205     // First find the best saving to start a new route.
3206     const Saving saving = savings_container_->GetSaving();
3207     int before_node = GetBeforeNodeFromSaving(saving);
3208     int after_node = GetAfterNodeFromSaving(saving);
3209     const bool nodes_not_contained =
3210         !Contains(before_node) && !Contains(after_node);
3211 
3212     bool committed = false;
3213 
3214     if (nodes_not_contained) {
3215       // Find the right vehicle to start the route with this Saving.
3216       const int type = GetVehicleTypeFromSaving(saving);
3217       const int vehicle =
3218           StartNewRouteWithBestVehicleOfType(type, before_node, after_node);
3219 
3220       if (vehicle >= 0) {
3221         committed = true;
3222         const int64_t start = model()->Start(vehicle);
3223         const int64_t end = model()->End(vehicle);
3224         // Then extend the route from both ends of the partial route.
3225         int in_index = 0;
3226         int out_index = 0;
3227         const int saving_offset = type * size;
3228 
3229         while (in_index < in_savings_ptr[saving_offset + after_node].size() ||
3230                out_index <
3231                    out_savings_ptr[saving_offset + before_node].size()) {
3232           if (StopSearch()) return;
3233           // First determine how to extend the route.
3234           int before_before_node = -1;
3235           int after_after_node = -1;
3236           if (in_index < in_savings_ptr[saving_offset + after_node].size()) {
3237             const Saving& in_saving =
3238                 *(in_savings_ptr[saving_offset + after_node][in_index]);
3239             if (out_index <
3240                 out_savings_ptr[saving_offset + before_node].size()) {
3241               const Saving& out_saving =
3242                   *(out_savings_ptr[saving_offset + before_node][out_index]);
3243               if (GetSavingValue(in_saving) < GetSavingValue(out_saving)) {
3244                 // Should extend after after_node
3245                 after_after_node = GetAfterNodeFromSaving(in_saving);
3246               } else {
3247                 // Should extend before before_node
3248                 before_before_node = GetBeforeNodeFromSaving(out_saving);
3249               }
3250             } else {
3251               // Should extend after after_node
3252               after_after_node = GetAfterNodeFromSaving(in_saving);
3253             }
3254           } else {
3255             // Should extend before before_node
3256             before_before_node = GetBeforeNodeFromSaving(
3257                 *(out_savings_ptr[saving_offset + before_node][out_index]));
3258           }
3259           // Extend the route
3260           if (after_after_node != -1) {
3261             DCHECK_EQ(before_before_node, -1);
3262             // Extending after after_node
3263             if (!Contains(after_after_node)) {
3264               SetValue(after_node, after_after_node);
3265               SetValue(after_after_node, end);
3266               if (Commit()) {
3267                 in_index = 0;
3268                 after_node = after_after_node;
3269               } else {
3270                 ++in_index;
3271               }
3272             } else {
3273               ++in_index;
3274             }
3275           } else {
3276             // Extending before before_node
3277             CHECK_GE(before_before_node, 0);
3278             if (!Contains(before_before_node)) {
3279               SetValue(start, before_before_node);
3280               SetValue(before_before_node, before_node);
3281               if (Commit()) {
3282                 out_index = 0;
3283                 before_node = before_before_node;
3284               } else {
3285                 ++out_index;
3286               }
3287             } else {
3288               ++out_index;
3289             }
3290           }
3291         }
3292       }
3293     }
3294     savings_container_->Update(nodes_not_contained && !committed);
3295   }
3296 }
3297 
3298 // ParallelSavingsFilteredHeuristic
3299 
BuildRoutesFromSavings()3300 void ParallelSavingsFilteredHeuristic::BuildRoutesFromSavings() {
3301   // Initialize the vehicles of the first/last non start/end nodes served by
3302   // each route.
3303   const int64_t size = model()->Size();
3304   const int vehicles = model()->vehicles();
3305 
3306   first_node_on_route_.resize(vehicles, -1);
3307   last_node_on_route_.resize(vehicles, -1);
3308   vehicle_of_first_or_last_node_.resize(size, -1);
3309 
3310   for (int vehicle = 0; vehicle < vehicles; vehicle++) {
3311     const int64_t start = model()->Start(vehicle);
3312     const int64_t end = model()->End(vehicle);
3313     if (!Contains(start)) {
3314       continue;
3315     }
3316     int64_t node = Value(start);
3317     if (node != end) {
3318       vehicle_of_first_or_last_node_[node] = vehicle;
3319       first_node_on_route_[vehicle] = node;
3320 
3321       int64_t next = Value(node);
3322       while (next != end) {
3323         node = next;
3324         next = Value(node);
3325       }
3326       vehicle_of_first_or_last_node_[node] = vehicle;
3327       last_node_on_route_[vehicle] = node;
3328     }
3329   }
3330 
3331   while (savings_container_->HasSaving()) {
3332     if (StopSearch()) return;
3333     const Saving saving = savings_container_->GetSaving();
3334     const int64_t before_node = GetBeforeNodeFromSaving(saving);
3335     const int64_t after_node = GetAfterNodeFromSaving(saving);
3336     const int type = GetVehicleTypeFromSaving(saving);
3337 
3338     if (!Contains(before_node) && !Contains(after_node)) {
3339       // Neither nodes are contained, start a new route.
3340       bool committed = false;
3341 
3342       const int vehicle =
3343           StartNewRouteWithBestVehicleOfType(type, before_node, after_node);
3344 
3345       if (vehicle >= 0) {
3346         committed = true;
3347         // Store before_node and after_node as first and last nodes of the route
3348         vehicle_of_first_or_last_node_[before_node] = vehicle;
3349         vehicle_of_first_or_last_node_[after_node] = vehicle;
3350         first_node_on_route_[vehicle] = before_node;
3351         last_node_on_route_[vehicle] = after_node;
3352         savings_container_->ReinjectSkippedSavingsStartingAt(after_node);
3353         savings_container_->ReinjectSkippedSavingsEndingAt(before_node);
3354       }
3355       savings_container_->Update(!committed);
3356       continue;
3357     }
3358 
3359     if (Contains(before_node) && Contains(after_node)) {
3360       // Merge the two routes if before_node is last and after_node first of its
3361       // route, the two nodes aren't already on the same route, and the vehicle
3362       // types are compatible.
3363       const int v1 = vehicle_of_first_or_last_node_[before_node];
3364       const int64_t last_node = v1 == -1 ? -1 : last_node_on_route_[v1];
3365 
3366       const int v2 = vehicle_of_first_or_last_node_[after_node];
3367       const int64_t first_node = v2 == -1 ? -1 : first_node_on_route_[v2];
3368 
3369       if (before_node == last_node && after_node == first_node && v1 != v2 &&
3370           vehicle_type_curator_->Type(v1) == vehicle_type_curator_->Type(v2)) {
3371         CHECK_EQ(Value(before_node), model()->End(v1));
3372         CHECK_EQ(Value(model()->Start(v2)), after_node);
3373 
3374         // We try merging the two routes.
3375         // TODO(user): Try to use skipped savings to start new routes when
3376         // a vehicle becomes available after a merge (not trivial because it can
3377         // result in an infinite loop).
3378         MergeRoutes(v1, v2, before_node, after_node);
3379       }
3380     }
3381 
3382     if (Contains(before_node) && !Contains(after_node)) {
3383       const int vehicle = vehicle_of_first_or_last_node_[before_node];
3384       const int64_t last_node =
3385           vehicle == -1 ? -1 : last_node_on_route_[vehicle];
3386 
3387       if (before_node == last_node) {
3388         const int64_t end = model()->End(vehicle);
3389         CHECK_EQ(Value(before_node), end);
3390 
3391         const int route_type = vehicle_type_curator_->Type(vehicle);
3392         if (type != route_type) {
3393           // The saving doesn't correspond to the type of the vehicle serving
3394           // before_node. We update the container with the correct type.
3395           savings_container_->UpdateWithType(route_type);
3396           continue;
3397         }
3398 
3399         // Try adding after_node on route of before_node.
3400         SetValue(before_node, after_node);
3401         SetValue(after_node, end);
3402         if (Commit()) {
3403           if (first_node_on_route_[vehicle] != before_node) {
3404             // before_node is no longer the start or end of its route
3405             DCHECK_NE(Value(model()->Start(vehicle)), before_node);
3406             vehicle_of_first_or_last_node_[before_node] = -1;
3407           }
3408           vehicle_of_first_or_last_node_[after_node] = vehicle;
3409           last_node_on_route_[vehicle] = after_node;
3410           savings_container_->ReinjectSkippedSavingsStartingAt(after_node);
3411         }
3412       }
3413     }
3414 
3415     if (!Contains(before_node) && Contains(after_node)) {
3416       const int vehicle = vehicle_of_first_or_last_node_[after_node];
3417       const int64_t first_node =
3418           vehicle == -1 ? -1 : first_node_on_route_[vehicle];
3419 
3420       if (after_node == first_node) {
3421         const int64_t start = model()->Start(vehicle);
3422         CHECK_EQ(Value(start), after_node);
3423 
3424         const int route_type = vehicle_type_curator_->Type(vehicle);
3425         if (type != route_type) {
3426           // The saving doesn't correspond to the type of the vehicle serving
3427           // after_node. We update the container with the correct type.
3428           savings_container_->UpdateWithType(route_type);
3429           continue;
3430         }
3431 
3432         // Try adding before_node on route of after_node.
3433         SetValue(before_node, after_node);
3434         SetValue(start, before_node);
3435         if (Commit()) {
3436           if (last_node_on_route_[vehicle] != after_node) {
3437             // after_node is no longer the start or end of its route
3438             DCHECK_NE(Value(after_node), model()->End(vehicle));
3439             vehicle_of_first_or_last_node_[after_node] = -1;
3440           }
3441           vehicle_of_first_or_last_node_[before_node] = vehicle;
3442           first_node_on_route_[vehicle] = before_node;
3443           savings_container_->ReinjectSkippedSavingsEndingAt(before_node);
3444         }
3445       }
3446     }
3447     savings_container_->Update(/*update_best_saving*/ false);
3448   }
3449 }
3450 
MergeRoutes(int first_vehicle,int second_vehicle,int64_t before_node,int64_t after_node)3451 void ParallelSavingsFilteredHeuristic::MergeRoutes(int first_vehicle,
3452                                                    int second_vehicle,
3453                                                    int64_t before_node,
3454                                                    int64_t after_node) {
3455   if (StopSearch()) return;
3456   const int64_t new_first_node = first_node_on_route_[first_vehicle];
3457   DCHECK_EQ(vehicle_of_first_or_last_node_[new_first_node], first_vehicle);
3458   CHECK_EQ(Value(model()->Start(first_vehicle)), new_first_node);
3459   const int64_t new_last_node = last_node_on_route_[second_vehicle];
3460   DCHECK_EQ(vehicle_of_first_or_last_node_[new_last_node], second_vehicle);
3461   CHECK_EQ(Value(new_last_node), model()->End(second_vehicle));
3462 
3463   // Select the vehicle with lower fixed cost to merge the routes.
3464   int used_vehicle = first_vehicle;
3465   int unused_vehicle = second_vehicle;
3466   if (model()->GetFixedCostOfVehicle(first_vehicle) >
3467       model()->GetFixedCostOfVehicle(second_vehicle)) {
3468     used_vehicle = second_vehicle;
3469     unused_vehicle = first_vehicle;
3470   }
3471 
3472   SetValue(before_node, after_node);
3473   SetValue(model()->Start(unused_vehicle), model()->End(unused_vehicle));
3474   if (used_vehicle == first_vehicle) {
3475     SetValue(new_last_node, model()->End(used_vehicle));
3476   } else {
3477     SetValue(model()->Start(used_vehicle), new_first_node);
3478   }
3479   bool committed = Commit();
3480   if (!committed &&
3481       model()->GetVehicleClassIndexOfVehicle(first_vehicle).value() !=
3482           model()->GetVehicleClassIndexOfVehicle(second_vehicle).value()) {
3483     // Try committing on other vehicle instead.
3484     std::swap(used_vehicle, unused_vehicle);
3485     SetValue(before_node, after_node);
3486     SetValue(model()->Start(unused_vehicle), model()->End(unused_vehicle));
3487     if (used_vehicle == first_vehicle) {
3488       SetValue(new_last_node, model()->End(used_vehicle));
3489     } else {
3490       SetValue(model()->Start(used_vehicle), new_first_node);
3491     }
3492     committed = Commit();
3493   }
3494   if (committed) {
3495     // Make unused_vehicle available
3496     vehicle_type_curator_->ReinjectVehicleOfClass(
3497         unused_vehicle,
3498         model()->GetVehicleClassIndexOfVehicle(unused_vehicle).value(),
3499         model()->GetFixedCostOfVehicle(unused_vehicle));
3500 
3501     // Update the first and last nodes on vehicles.
3502     first_node_on_route_[unused_vehicle] = -1;
3503     last_node_on_route_[unused_vehicle] = -1;
3504     vehicle_of_first_or_last_node_[before_node] = -1;
3505     vehicle_of_first_or_last_node_[after_node] = -1;
3506     first_node_on_route_[used_vehicle] = new_first_node;
3507     last_node_on_route_[used_vehicle] = new_last_node;
3508     vehicle_of_first_or_last_node_[new_last_node] = used_vehicle;
3509     vehicle_of_first_or_last_node_[new_first_node] = used_vehicle;
3510   }
3511 }
3512 
3513 // ChristofidesFilteredHeuristic
3514 
ChristofidesFilteredHeuristic(RoutingModel * model,LocalSearchFilterManager * filter_manager,bool use_minimum_matching)3515 ChristofidesFilteredHeuristic::ChristofidesFilteredHeuristic(
3516     RoutingModel* model, LocalSearchFilterManager* filter_manager,
3517     bool use_minimum_matching)
3518     : RoutingFilteredHeuristic(model, filter_manager),
3519       use_minimum_matching_(use_minimum_matching) {}
3520 
3521 // TODO(user): Support pickup & delivery.
BuildSolutionInternal()3522 bool ChristofidesFilteredHeuristic::BuildSolutionInternal() {
3523   const int size = model()->Size() - model()->vehicles() + 1;
3524   // Node indices for Christofides solver.
3525   // 0: start/end node
3526   // >0: non start/end nodes
3527   // TODO(user): Add robustness to fixed arcs by collapsing them into meta-
3528   // nodes.
3529   std::vector<int> indices(1, 0);
3530   for (int i = 1; i < size; ++i) {
3531     if (!model()->IsStart(i) && !model()->IsEnd(i)) {
3532       indices.push_back(i);
3533     }
3534   }
3535   const int num_cost_classes = model()->GetCostClassesCount();
3536   std::vector<std::vector<int>> path_per_cost_class(num_cost_classes);
3537   std::vector<bool> class_covered(num_cost_classes, false);
3538   for (int vehicle = 0; vehicle < model()->vehicles(); ++vehicle) {
3539     const int64_t cost_class =
3540         model()->GetCostClassIndexOfVehicle(vehicle).value();
3541     if (!class_covered[cost_class]) {
3542       class_covered[cost_class] = true;
3543       const int64_t start = model()->Start(vehicle);
3544       const int64_t end = model()->End(vehicle);
3545       auto cost = [this, &indices, start, end, cost_class](int from, int to) {
3546         DCHECK_LT(from, indices.size());
3547         DCHECK_LT(to, indices.size());
3548         const int from_index = (from == 0) ? start : indices[from];
3549         const int to_index = (to == 0) ? end : indices[to];
3550         const int64_t cost =
3551             model()->GetArcCostForClass(from_index, to_index, cost_class);
3552         // To avoid overflow issues, capping costs at kint64max/2, the maximum
3553         // value supported by MinCostPerfectMatching.
3554         // TODO(user): Investigate if ChristofidesPathSolver should not
3555         // return a status to bail out fast in case of problem.
3556         return std::min(cost, std::numeric_limits<int64_t>::max() / 2);
3557       };
3558       using Cost = decltype(cost);
3559       ChristofidesPathSolver<int64_t, int64_t, int, Cost> christofides_solver(
3560           indices.size(), cost);
3561       if (use_minimum_matching_) {
3562         christofides_solver.SetMatchingAlgorithm(
3563             ChristofidesPathSolver<int64_t, int64_t, int, Cost>::
3564                 MatchingAlgorithm::MINIMUM_WEIGHT_MATCHING);
3565       }
3566       if (christofides_solver.Solve()) {
3567         path_per_cost_class[cost_class] =
3568             christofides_solver.TravelingSalesmanPath();
3569       }
3570     }
3571   }
3572   // TODO(user): Investigate if sorting paths per cost improves solutions.
3573   for (int vehicle = 0; vehicle < model()->vehicles(); ++vehicle) {
3574     const int64_t cost_class =
3575         model()->GetCostClassIndexOfVehicle(vehicle).value();
3576     const std::vector<int>& path = path_per_cost_class[cost_class];
3577     if (path.empty()) continue;
3578     DCHECK_EQ(0, path[0]);
3579     DCHECK_EQ(0, path.back());
3580     // Extend route from start.
3581     int prev = GetStartChainEnd(vehicle);
3582     const int end = model()->End(vehicle);
3583     for (int i = 1; i < path.size() - 1 && prev != end; ++i) {
3584       if (StopSearch()) return false;
3585       int next = indices[path[i]];
3586       if (!Contains(next)) {
3587         SetValue(prev, next);
3588         SetValue(next, end);
3589         if (Commit()) {
3590           prev = next;
3591         }
3592       }
3593     }
3594   }
3595   MakeUnassignedNodesUnperformed();
3596   return Commit();
3597 }
3598 
3599 // Sweep heuristic
3600 // TODO(user): Clean up to match other first solution strategies.
3601 
3602 namespace {
3603 struct SweepIndex {
SweepIndexoperations_research::__anona5ac6c7b1b11::SweepIndex3604   SweepIndex(const int64_t index, const double angle, const double distance)
3605       : index(index), angle(angle), distance(distance) {}
~SweepIndexoperations_research::__anona5ac6c7b1b11::SweepIndex3606   ~SweepIndex() {}
3607 
3608   int64_t index;
3609   double angle;
3610   double distance;
3611 };
3612 
3613 struct SweepIndexSortAngle {
operator ()operations_research::__anona5ac6c7b1b11::SweepIndexSortAngle3614   bool operator()(const SweepIndex& node1, const SweepIndex& node2) const {
3615     return (node1.angle < node2.angle);
3616   }
3617 } SweepIndexAngleComparator;
3618 
3619 struct SweepIndexSortDistance {
operator ()operations_research::__anona5ac6c7b1b11::SweepIndexSortDistance3620   bool operator()(const SweepIndex& node1, const SweepIndex& node2) const {
3621     return (node1.distance < node2.distance);
3622   }
3623 } SweepIndexDistanceComparator;
3624 }  // namespace
3625 
SweepArranger(const std::vector<std::pair<int64_t,int64_t>> & points)3626 SweepArranger::SweepArranger(
3627     const std::vector<std::pair<int64_t, int64_t>>& points)
3628     : coordinates_(2 * points.size(), 0), sectors_(1) {
3629   for (int64_t i = 0; i < points.size(); ++i) {
3630     coordinates_[2 * i] = points[i].first;
3631     coordinates_[2 * i + 1] = points[i].second;
3632   }
3633 }
3634 
3635 // Splits the space of the indices into sectors and sorts the indices of each
3636 // sector with ascending angle from the depot.
ArrangeIndices(std::vector<int64_t> * indices)3637 void SweepArranger::ArrangeIndices(std::vector<int64_t>* indices) {
3638   const double pi_rad = 3.14159265;
3639   // Suppose that the center is at x0, y0.
3640   const int x0 = coordinates_[0];
3641   const int y0 = coordinates_[1];
3642 
3643   std::vector<SweepIndex> sweep_indices;
3644   for (int64_t index = 0; index < static_cast<int>(coordinates_.size()) / 2;
3645        ++index) {
3646     const int x = coordinates_[2 * index];
3647     const int y = coordinates_[2 * index + 1];
3648     const double x_delta = x - x0;
3649     const double y_delta = y - y0;
3650     double square_distance = x_delta * x_delta + y_delta * y_delta;
3651     double angle = square_distance == 0 ? 0 : std::atan2(y_delta, x_delta);
3652     angle = angle >= 0 ? angle : 2 * pi_rad + angle;
3653     SweepIndex sweep_index(index, angle, square_distance);
3654     sweep_indices.push_back(sweep_index);
3655   }
3656   std::sort(sweep_indices.begin(), sweep_indices.end(),
3657             SweepIndexDistanceComparator);
3658 
3659   const int size = static_cast<int>(sweep_indices.size()) / sectors_;
3660   for (int sector = 0; sector < sectors_; ++sector) {
3661     std::vector<SweepIndex> cluster;
3662     std::vector<SweepIndex>::iterator begin =
3663         sweep_indices.begin() + sector * size;
3664     std::vector<SweepIndex>::iterator end =
3665         sector == sectors_ - 1 ? sweep_indices.end()
3666                                : sweep_indices.begin() + (sector + 1) * size;
3667     std::sort(begin, end, SweepIndexAngleComparator);
3668   }
3669   for (const SweepIndex& sweep_index : sweep_indices) {
3670     indices->push_back(sweep_index.index);
3671   }
3672 }
3673 
3674 namespace {
3675 
3676 struct Link {
Linkoperations_research::__anona5ac6c7b1c11::Link3677   Link(std::pair<int, int> link, double value, int vehicle_class,
3678        int64_t start_depot, int64_t end_depot)
3679       : link(link),
3680         value(value),
3681         vehicle_class(vehicle_class),
3682         start_depot(start_depot),
3683         end_depot(end_depot) {}
~Linkoperations_research::__anona5ac6c7b1c11::Link3684   ~Link() {}
3685 
3686   std::pair<int, int> link;
3687   int64_t value;
3688   int vehicle_class;
3689   int64_t start_depot;
3690   int64_t end_depot;
3691 };
3692 
3693 // The RouteConstructor creates the routes of a VRP instance subject to its
3694 // constraints by iterating on a list of arcs appearing in descending order
3695 // of priority.
3696 // TODO(user): Use the dimension class in this class.
3697 // TODO(user): Add support for vehicle-dependent dimension transits.
3698 class RouteConstructor {
3699  public:
RouteConstructor(Assignment * const assignment,RoutingModel * const model,bool check_assignment,int64_t num_indices,const std::vector<Link> & links_list)3700   RouteConstructor(Assignment* const assignment, RoutingModel* const model,
3701                    bool check_assignment, int64_t num_indices,
3702                    const std::vector<Link>& links_list)
3703       : assignment_(assignment),
3704         model_(model),
3705         check_assignment_(check_assignment),
3706         solver_(model_->solver()),
3707         num_indices_(num_indices),
3708         links_list_(links_list),
3709         nexts_(model_->Nexts()),
3710         in_route_(num_indices_, -1),
3711         final_routes_(),
3712         index_to_chain_index_(num_indices, -1),
3713         index_to_vehicle_class_index_(num_indices, -1) {
3714     {
3715       const std::vector<std::string> dimension_names =
3716           model_->GetAllDimensionNames();
3717       dimensions_.assign(dimension_names.size(), nullptr);
3718       for (int i = 0; i < dimension_names.size(); ++i) {
3719         dimensions_[i] = &model_->GetDimensionOrDie(dimension_names[i]);
3720       }
3721     }
3722     cumuls_.resize(dimensions_.size());
3723     for (std::vector<int64_t>& cumuls : cumuls_) {
3724       cumuls.resize(num_indices_);
3725     }
3726     new_possible_cumuls_.resize(dimensions_.size());
3727   }
3728 
~RouteConstructor()3729   ~RouteConstructor() {}
3730 
Construct()3731   void Construct() {
3732     model_->solver()->TopPeriodicCheck();
3733     // Initial State: Each order is served by its own vehicle.
3734     for (int index = 0; index < num_indices_; ++index) {
3735       if (!model_->IsStart(index) && !model_->IsEnd(index)) {
3736         std::vector<int> route(1, index);
3737         routes_.push_back(route);
3738         in_route_[index] = routes_.size() - 1;
3739       }
3740     }
3741 
3742     for (const Link& link : links_list_) {
3743       model_->solver()->TopPeriodicCheck();
3744       const int index1 = link.link.first;
3745       const int index2 = link.link.second;
3746       const int vehicle_class = link.vehicle_class;
3747       const int64_t start_depot = link.start_depot;
3748       const int64_t end_depot = link.end_depot;
3749 
3750       // Initialisation of cumuls_ if the indices are encountered for first time
3751       if (index_to_vehicle_class_index_[index1] < 0) {
3752         for (int dimension_index = 0; dimension_index < dimensions_.size();
3753              ++dimension_index) {
3754           cumuls_[dimension_index][index1] =
3755               std::max(dimensions_[dimension_index]->GetTransitValue(
3756                            start_depot, index1, 0),
3757                        dimensions_[dimension_index]->CumulVar(index1)->Min());
3758         }
3759       }
3760       if (index_to_vehicle_class_index_[index2] < 0) {
3761         for (int dimension_index = 0; dimension_index < dimensions_.size();
3762              ++dimension_index) {
3763           cumuls_[dimension_index][index2] =
3764               std::max(dimensions_[dimension_index]->GetTransitValue(
3765                            start_depot, index2, 0),
3766                        dimensions_[dimension_index]->CumulVar(index2)->Min());
3767         }
3768       }
3769 
3770       const int route_index1 = in_route_[index1];
3771       const int route_index2 = in_route_[index2];
3772       const bool merge =
3773           route_index1 >= 0 && route_index2 >= 0 &&
3774           FeasibleMerge(routes_[route_index1], routes_[route_index2], index1,
3775                         index2, route_index1, route_index2, vehicle_class,
3776                         start_depot, end_depot);
3777       if (Merge(merge, route_index1, route_index2)) {
3778         index_to_vehicle_class_index_[index1] = vehicle_class;
3779         index_to_vehicle_class_index_[index2] = vehicle_class;
3780       }
3781     }
3782 
3783     model_->solver()->TopPeriodicCheck();
3784     // Beyond this point not checking limits anymore as the rest of the code is
3785     // linear and that given we managed to build a solution would be stupid to
3786     // drop it now.
3787     for (int chain_index = 0; chain_index < chains_.size(); ++chain_index) {
3788       if (!deleted_chains_.contains(chain_index)) {
3789         final_chains_.push_back(chains_[chain_index]);
3790       }
3791     }
3792     std::sort(final_chains_.begin(), final_chains_.end(), ChainComparator);
3793     for (int route_index = 0; route_index < routes_.size(); ++route_index) {
3794       if (!deleted_routes_.contains(route_index)) {
3795         final_routes_.push_back(routes_[route_index]);
3796       }
3797     }
3798     std::sort(final_routes_.begin(), final_routes_.end(), RouteComparator);
3799 
3800     const int extra_vehicles = std::max(
3801         0, static_cast<int>(final_chains_.size()) - model_->vehicles());
3802     // Bind the Start and End of each chain
3803     int chain_index = 0;
3804     for (chain_index = extra_vehicles; chain_index < final_chains_.size();
3805          ++chain_index) {
3806       if (chain_index - extra_vehicles >= model_->vehicles()) {
3807         break;
3808       }
3809       const int start = final_chains_[chain_index].head;
3810       const int end = final_chains_[chain_index].tail;
3811       assignment_->Add(
3812           model_->NextVar(model_->Start(chain_index - extra_vehicles)));
3813       assignment_->SetValue(
3814           model_->NextVar(model_->Start(chain_index - extra_vehicles)), start);
3815       assignment_->Add(nexts_[end]);
3816       assignment_->SetValue(nexts_[end],
3817                             model_->End(chain_index - extra_vehicles));
3818     }
3819 
3820     // Create the single order routes
3821     for (int route_index = 0; route_index < final_routes_.size();
3822          ++route_index) {
3823       if (chain_index - extra_vehicles >= model_->vehicles()) {
3824         break;
3825       }
3826       DCHECK_LT(route_index, final_routes_.size());
3827       const int head = final_routes_[route_index].front();
3828       const int tail = final_routes_[route_index].back();
3829       if (head == tail && head < model_->Size()) {
3830         assignment_->Add(
3831             model_->NextVar(model_->Start(chain_index - extra_vehicles)));
3832         assignment_->SetValue(
3833             model_->NextVar(model_->Start(chain_index - extra_vehicles)), head);
3834         assignment_->Add(nexts_[tail]);
3835         assignment_->SetValue(nexts_[tail],
3836                               model_->End(chain_index - extra_vehicles));
3837         ++chain_index;
3838       }
3839     }
3840 
3841     // Unperformed
3842     for (int index = 0; index < model_->Size(); ++index) {
3843       IntVar* const next = nexts_[index];
3844       if (!assignment_->Contains(next)) {
3845         assignment_->Add(next);
3846         if (next->Contains(index)) {
3847           assignment_->SetValue(next, index);
3848         }
3849       }
3850     }
3851   }
3852 
3853  private:
3854   enum MergeStatus { FIRST_SECOND, SECOND_FIRST, NO_MERGE };
3855 
3856   struct RouteSort {
operator ()operations_research::__anona5ac6c7b1c11::RouteConstructor::RouteSort3857     bool operator()(const std::vector<int>& route1,
3858                     const std::vector<int>& route2) const {
3859       return (route1.size() < route2.size());
3860     }
3861   } RouteComparator;
3862 
3863   struct Chain {
3864     int head;
3865     int tail;
3866     int nodes;
3867   };
3868 
3869   struct ChainSort {
operator ()operations_research::__anona5ac6c7b1c11::RouteConstructor::ChainSort3870     bool operator()(const Chain& chain1, const Chain& chain2) const {
3871       return (chain1.nodes < chain2.nodes);
3872     }
3873   } ChainComparator;
3874 
Head(int node) const3875   bool Head(int node) const {
3876     return (node == routes_[in_route_[node]].front());
3877   }
3878 
Tail(int node) const3879   bool Tail(int node) const {
3880     return (node == routes_[in_route_[node]].back());
3881   }
3882 
FeasibleRoute(const std::vector<int> & route,int64_t route_cumul,int dimension_index)3883   bool FeasibleRoute(const std::vector<int>& route, int64_t route_cumul,
3884                      int dimension_index) {
3885     const RoutingDimension& dimension = *dimensions_[dimension_index];
3886     std::vector<int>::const_iterator it = route.begin();
3887     int64_t cumul = route_cumul;
3888     while (it != route.end()) {
3889       const int previous = *it;
3890       const int64_t cumul_previous = cumul;
3891       gtl::InsertOrDie(&(new_possible_cumuls_[dimension_index]), previous,
3892                        cumul_previous);
3893       ++it;
3894       if (it == route.end()) {
3895         return true;
3896       }
3897       const int next = *it;
3898       int64_t available_from_previous =
3899           cumul_previous + dimension.GetTransitValue(previous, next, 0);
3900       int64_t available_cumul_next =
3901           std::max(cumuls_[dimension_index][next], available_from_previous);
3902 
3903       const int64_t slack = available_cumul_next - available_from_previous;
3904       if (slack > dimension.SlackVar(previous)->Max()) {
3905         available_cumul_next =
3906             available_from_previous + dimension.SlackVar(previous)->Max();
3907       }
3908 
3909       if (available_cumul_next > dimension.CumulVar(next)->Max()) {
3910         return false;
3911       }
3912       if (available_cumul_next <= cumuls_[dimension_index][next]) {
3913         return true;
3914       }
3915       cumul = available_cumul_next;
3916     }
3917     return true;
3918   }
3919 
CheckRouteConnection(const std::vector<int> & route1,const std::vector<int> & route2,int dimension_index,int64_t start_depot,int64_t end_depot)3920   bool CheckRouteConnection(const std::vector<int>& route1,
3921                             const std::vector<int>& route2, int dimension_index,
3922                             int64_t start_depot, int64_t end_depot) {
3923     const int tail1 = route1.back();
3924     const int head2 = route2.front();
3925     const int tail2 = route2.back();
3926     const RoutingDimension& dimension = *dimensions_[dimension_index];
3927     int non_depot_node = -1;
3928     for (int node = 0; node < num_indices_; ++node) {
3929       if (!model_->IsStart(node) && !model_->IsEnd(node)) {
3930         non_depot_node = node;
3931         break;
3932       }
3933     }
3934     CHECK_GE(non_depot_node, 0);
3935     const int64_t depot_threshold =
3936         std::max(dimension.SlackVar(non_depot_node)->Max(),
3937                  dimension.CumulVar(non_depot_node)->Max());
3938 
3939     int64_t available_from_tail1 = cumuls_[dimension_index][tail1] +
3940                                    dimension.GetTransitValue(tail1, head2, 0);
3941     int64_t new_available_cumul_head2 =
3942         std::max(cumuls_[dimension_index][head2], available_from_tail1);
3943 
3944     const int64_t slack = new_available_cumul_head2 - available_from_tail1;
3945     if (slack > dimension.SlackVar(tail1)->Max()) {
3946       new_available_cumul_head2 =
3947           available_from_tail1 + dimension.SlackVar(tail1)->Max();
3948     }
3949 
3950     bool feasible_route = true;
3951     if (new_available_cumul_head2 > dimension.CumulVar(head2)->Max()) {
3952       return false;
3953     }
3954     if (new_available_cumul_head2 <= cumuls_[dimension_index][head2]) {
3955       return true;
3956     }
3957 
3958     feasible_route =
3959         FeasibleRoute(route2, new_available_cumul_head2, dimension_index);
3960     const int64_t new_possible_cumul_tail2 =
3961         new_possible_cumuls_[dimension_index].contains(tail2)
3962             ? new_possible_cumuls_[dimension_index][tail2]
3963             : cumuls_[dimension_index][tail2];
3964 
3965     if (!feasible_route || (new_possible_cumul_tail2 +
3966                                 dimension.GetTransitValue(tail2, end_depot, 0) >
3967                             depot_threshold)) {
3968       return false;
3969     }
3970     return true;
3971   }
3972 
FeasibleMerge(const std::vector<int> & route1,const std::vector<int> & route2,int node1,int node2,int route_index1,int route_index2,int vehicle_class,int64_t start_depot,int64_t end_depot)3973   bool FeasibleMerge(const std::vector<int>& route1,
3974                      const std::vector<int>& route2, int node1, int node2,
3975                      int route_index1, int route_index2, int vehicle_class,
3976                      int64_t start_depot, int64_t end_depot) {
3977     if ((route_index1 == route_index2) || !(Tail(node1) && Head(node2))) {
3978       return false;
3979     }
3980 
3981     // Vehicle Class Check
3982     if (!((index_to_vehicle_class_index_[node1] == -1 &&
3983            index_to_vehicle_class_index_[node2] == -1) ||
3984           (index_to_vehicle_class_index_[node1] == vehicle_class &&
3985            index_to_vehicle_class_index_[node2] == -1) ||
3986           (index_to_vehicle_class_index_[node1] == -1 &&
3987            index_to_vehicle_class_index_[node2] == vehicle_class) ||
3988           (index_to_vehicle_class_index_[node1] == vehicle_class &&
3989            index_to_vehicle_class_index_[node2] == vehicle_class))) {
3990       return false;
3991     }
3992 
3993     // Check Route1 -> Route2 connection for every dimension
3994     bool merge = true;
3995     for (int dimension_index = 0; dimension_index < dimensions_.size();
3996          ++dimension_index) {
3997       new_possible_cumuls_[dimension_index].clear();
3998       merge = merge && CheckRouteConnection(route1, route2, dimension_index,
3999                                             start_depot, end_depot);
4000       if (!merge) {
4001         return false;
4002       }
4003     }
4004     return true;
4005   }
4006 
CheckTempAssignment(Assignment * const temp_assignment,int new_chain_index,int old_chain_index,int head1,int tail1,int head2,int tail2)4007   bool CheckTempAssignment(Assignment* const temp_assignment,
4008                            int new_chain_index, int old_chain_index, int head1,
4009                            int tail1, int head2, int tail2) {
4010     // TODO(user): If the chain index is greater than the number of vehicles,
4011     // use another vehicle instead.
4012     if (new_chain_index >= model_->vehicles()) return false;
4013     const int start = head1;
4014     temp_assignment->Add(model_->NextVar(model_->Start(new_chain_index)));
4015     temp_assignment->SetValue(model_->NextVar(model_->Start(new_chain_index)),
4016                               start);
4017     temp_assignment->Add(nexts_[tail1]);
4018     temp_assignment->SetValue(nexts_[tail1], head2);
4019     temp_assignment->Add(nexts_[tail2]);
4020     temp_assignment->SetValue(nexts_[tail2], model_->End(new_chain_index));
4021     for (int chain_index = 0; chain_index < chains_.size(); ++chain_index) {
4022       if ((chain_index != new_chain_index) &&
4023           (chain_index != old_chain_index) &&
4024           (!deleted_chains_.contains(chain_index))) {
4025         const int start = chains_[chain_index].head;
4026         const int end = chains_[chain_index].tail;
4027         temp_assignment->Add(model_->NextVar(model_->Start(chain_index)));
4028         temp_assignment->SetValue(model_->NextVar(model_->Start(chain_index)),
4029                                   start);
4030         temp_assignment->Add(nexts_[end]);
4031         temp_assignment->SetValue(nexts_[end], model_->End(chain_index));
4032       }
4033     }
4034     return solver_->Solve(solver_->MakeRestoreAssignment(temp_assignment));
4035   }
4036 
UpdateAssignment(const std::vector<int> & route1,const std::vector<int> & route2)4037   bool UpdateAssignment(const std::vector<int>& route1,
4038                         const std::vector<int>& route2) {
4039     bool feasible = true;
4040     const int head1 = route1.front();
4041     const int tail1 = route1.back();
4042     const int head2 = route2.front();
4043     const int tail2 = route2.back();
4044     const int chain_index1 = index_to_chain_index_[head1];
4045     const int chain_index2 = index_to_chain_index_[head2];
4046     if (chain_index1 < 0 && chain_index2 < 0) {
4047       const int chain_index = chains_.size();
4048       if (check_assignment_) {
4049         Assignment* const temp_assignment =
4050             solver_->MakeAssignment(assignment_);
4051         feasible = CheckTempAssignment(temp_assignment, chain_index, -1, head1,
4052                                        tail1, head2, tail2);
4053       }
4054       if (feasible) {
4055         Chain chain;
4056         chain.head = head1;
4057         chain.tail = tail2;
4058         chain.nodes = 2;
4059         index_to_chain_index_[head1] = chain_index;
4060         index_to_chain_index_[tail2] = chain_index;
4061         chains_.push_back(chain);
4062       }
4063     } else if (chain_index1 >= 0 && chain_index2 < 0) {
4064       if (check_assignment_) {
4065         Assignment* const temp_assignment =
4066             solver_->MakeAssignment(assignment_);
4067         feasible =
4068             CheckTempAssignment(temp_assignment, chain_index1, chain_index2,
4069                                 head1, tail1, head2, tail2);
4070       }
4071       if (feasible) {
4072         index_to_chain_index_[tail2] = chain_index1;
4073         chains_[chain_index1].head = head1;
4074         chains_[chain_index1].tail = tail2;
4075         ++chains_[chain_index1].nodes;
4076       }
4077     } else if (chain_index1 < 0 && chain_index2 >= 0) {
4078       if (check_assignment_) {
4079         Assignment* const temp_assignment =
4080             solver_->MakeAssignment(assignment_);
4081         feasible =
4082             CheckTempAssignment(temp_assignment, chain_index2, chain_index1,
4083                                 head1, tail1, head2, tail2);
4084       }
4085       if (feasible) {
4086         index_to_chain_index_[head1] = chain_index2;
4087         chains_[chain_index2].head = head1;
4088         chains_[chain_index2].tail = tail2;
4089         ++chains_[chain_index2].nodes;
4090       }
4091     } else {
4092       if (check_assignment_) {
4093         Assignment* const temp_assignment =
4094             solver_->MakeAssignment(assignment_);
4095         feasible =
4096             CheckTempAssignment(temp_assignment, chain_index1, chain_index2,
4097                                 head1, tail1, head2, tail2);
4098       }
4099       if (feasible) {
4100         index_to_chain_index_[tail2] = chain_index1;
4101         chains_[chain_index1].head = head1;
4102         chains_[chain_index1].tail = tail2;
4103         chains_[chain_index1].nodes += chains_[chain_index2].nodes;
4104         deleted_chains_.insert(chain_index2);
4105       }
4106     }
4107     if (feasible) {
4108       assignment_->Add(nexts_[tail1]);
4109       assignment_->SetValue(nexts_[tail1], head2);
4110     }
4111     return feasible;
4112   }
4113 
Merge(bool merge,int index1,int index2)4114   bool Merge(bool merge, int index1, int index2) {
4115     if (merge) {
4116       if (UpdateAssignment(routes_[index1], routes_[index2])) {
4117         // Connection Route1 -> Route2
4118         for (const int node : routes_[index2]) {
4119           in_route_[node] = index1;
4120           routes_[index1].push_back(node);
4121         }
4122         for (int dimension_index = 0; dimension_index < dimensions_.size();
4123              ++dimension_index) {
4124           for (const std::pair<int, int64_t> new_possible_cumul :
4125                new_possible_cumuls_[dimension_index]) {
4126             cumuls_[dimension_index][new_possible_cumul.first] =
4127                 new_possible_cumul.second;
4128           }
4129         }
4130         deleted_routes_.insert(index2);
4131         return true;
4132       }
4133     }
4134     return false;
4135   }
4136 
4137   Assignment* const assignment_;
4138   RoutingModel* const model_;
4139   const bool check_assignment_;
4140   Solver* const solver_;
4141   const int64_t num_indices_;
4142   const std::vector<Link> links_list_;
4143   std::vector<IntVar*> nexts_;
4144   std::vector<const RoutingDimension*> dimensions_;  // Not owned.
4145   std::vector<std::vector<int64_t>> cumuls_;
4146   std::vector<absl::flat_hash_map<int, int64_t>> new_possible_cumuls_;
4147   std::vector<std::vector<int>> routes_;
4148   std::vector<int> in_route_;
4149   absl::flat_hash_set<int> deleted_routes_;
4150   std::vector<std::vector<int>> final_routes_;
4151   std::vector<Chain> chains_;
4152   absl::flat_hash_set<int> deleted_chains_;
4153   std::vector<Chain> final_chains_;
4154   std::vector<int> index_to_chain_index_;
4155   std::vector<int> index_to_vehicle_class_index_;
4156 };
4157 
4158 // Decision Builder building a first solution based on Sweep heuristic for
4159 // Vehicle Routing Problem.
4160 // Suitable only when distance is considered as the cost.
4161 class SweepBuilder : public DecisionBuilder {
4162  public:
SweepBuilder(RoutingModel * const model,bool check_assignment)4163   SweepBuilder(RoutingModel* const model, bool check_assignment)
4164       : model_(model), check_assignment_(check_assignment) {}
~SweepBuilder()4165   ~SweepBuilder() override {}
4166 
Next(Solver * const solver)4167   Decision* Next(Solver* const solver) override {
4168     // Setup the model of the instance for the Sweep Algorithm
4169     ModelSetup();
4170 
4171     // Build the assignment routes for the model
4172     Assignment* const assignment = solver->MakeAssignment();
4173     route_constructor_ = absl::make_unique<RouteConstructor>(
4174         assignment, model_, check_assignment_, num_indices_, links_);
4175     // This call might cause backtracking if the search limit is reached.
4176     route_constructor_->Construct();
4177     route_constructor_.reset(nullptr);
4178     // This call might cause backtracking if the solution is not feasible.
4179     assignment->Restore();
4180 
4181     return nullptr;
4182   }
4183 
4184  private:
ModelSetup()4185   void ModelSetup() {
4186     const int depot = model_->GetDepot();
4187     num_indices_ = model_->Size() + model_->vehicles();
4188     if (absl::GetFlag(FLAGS_sweep_sectors) > 0 &&
4189         absl::GetFlag(FLAGS_sweep_sectors) < num_indices_) {
4190       model_->sweep_arranger()->SetSectors(absl::GetFlag(FLAGS_sweep_sectors));
4191     }
4192     std::vector<int64_t> indices;
4193     model_->sweep_arranger()->ArrangeIndices(&indices);
4194     for (int i = 0; i < indices.size() - 1; ++i) {
4195       const int64_t first = indices[i];
4196       const int64_t second = indices[i + 1];
4197       if ((model_->IsStart(first) || !model_->IsEnd(first)) &&
4198           (model_->IsStart(second) || !model_->IsEnd(second))) {
4199         if (first != depot && second != depot) {
4200           Link link(std::make_pair(first, second), 0, 0, depot, depot);
4201           links_.push_back(link);
4202         }
4203       }
4204     }
4205   }
4206 
4207   RoutingModel* const model_;
4208   std::unique_ptr<RouteConstructor> route_constructor_;
4209   const bool check_assignment_;
4210   int64_t num_indices_;
4211   std::vector<Link> links_;
4212 };
4213 }  // namespace
4214 
MakeSweepDecisionBuilder(RoutingModel * model,bool check_assignment)4215 DecisionBuilder* MakeSweepDecisionBuilder(RoutingModel* model,
4216                                           bool check_assignment) {
4217   return model->solver()->RevAlloc(new SweepBuilder(model, check_assignment));
4218 }
4219 
4220 // AllUnperformed
4221 
4222 namespace {
4223 // Decision builder to build a solution with all nodes inactive. It does no
4224 // branching and may fail if some nodes cannot be made inactive.
4225 
4226 class AllUnperformed : public DecisionBuilder {
4227  public:
4228   // Does not take ownership of model.
AllUnperformed(RoutingModel * const model)4229   explicit AllUnperformed(RoutingModel* const model) : model_(model) {}
~AllUnperformed()4230   ~AllUnperformed() override {}
Next(Solver * const solver)4231   Decision* Next(Solver* const solver) override {
4232     // Solver::(Un)FreezeQueue is private, passing through the public API
4233     // on PropagationBaseObject.
4234     model_->CostVar()->FreezeQueue();
4235     for (int i = 0; i < model_->Size(); ++i) {
4236       if (!model_->IsStart(i)) {
4237         model_->ActiveVar(i)->SetValue(0);
4238       }
4239     }
4240     model_->CostVar()->UnfreezeQueue();
4241     return nullptr;
4242   }
4243 
4244  private:
4245   RoutingModel* const model_;
4246 };
4247 }  // namespace
4248 
MakeAllUnperformed(RoutingModel * model)4249 DecisionBuilder* MakeAllUnperformed(RoutingModel* model) {
4250   return model->solver()->RevAlloc(new AllUnperformed(model));
4251 }
4252 
4253 namespace {
4254 // The description is in routing.h:MakeGuidedSlackFinalizer
4255 class GuidedSlackFinalizer : public DecisionBuilder {
4256  public:
4257   GuidedSlackFinalizer(const RoutingDimension* dimension, RoutingModel* model,
4258                        std::function<int64_t(int64_t)> initializer);
4259   Decision* Next(Solver* solver) override;
4260 
4261  private:
4262   int64_t SelectValue(int64_t index);
4263   int64_t ChooseVariable();
4264 
4265   const RoutingDimension* const dimension_;
4266   RoutingModel* const model_;
4267   const std::function<int64_t(int64_t)> initializer_;
4268   RevArray<bool> is_initialized_;
4269   std::vector<int64_t> initial_values_;
4270   Rev<int64_t> current_index_;
4271   Rev<int64_t> current_route_;
4272   RevArray<int64_t> last_delta_used_;
4273 
4274   DISALLOW_COPY_AND_ASSIGN(GuidedSlackFinalizer);
4275 };
4276 
GuidedSlackFinalizer(const RoutingDimension * dimension,RoutingModel * model,std::function<int64_t (int64_t)> initializer)4277 GuidedSlackFinalizer::GuidedSlackFinalizer(
4278     const RoutingDimension* dimension, RoutingModel* model,
4279     std::function<int64_t(int64_t)> initializer)
4280     : dimension_(ABSL_DIE_IF_NULL(dimension)),
4281       model_(ABSL_DIE_IF_NULL(model)),
4282       initializer_(std::move(initializer)),
4283       is_initialized_(dimension->slacks().size(), false),
4284       initial_values_(dimension->slacks().size(),
4285                       std::numeric_limits<int64_t>::min()),
4286       current_index_(model_->Start(0)),
4287       current_route_(0),
4288       last_delta_used_(dimension->slacks().size(), 0) {}
4289 
Next(Solver * solver)4290 Decision* GuidedSlackFinalizer::Next(Solver* solver) {
4291   CHECK_EQ(solver, model_->solver());
4292   const int node_idx = ChooseVariable();
4293   CHECK(node_idx == -1 ||
4294         (node_idx >= 0 && node_idx < dimension_->slacks().size()));
4295   if (node_idx != -1) {
4296     if (!is_initialized_[node_idx]) {
4297       initial_values_[node_idx] = initializer_(node_idx);
4298       is_initialized_.SetValue(solver, node_idx, true);
4299     }
4300     const int64_t value = SelectValue(node_idx);
4301     IntVar* const slack_variable = dimension_->SlackVar(node_idx);
4302     return solver->MakeAssignVariableValue(slack_variable, value);
4303   }
4304   return nullptr;
4305 }
4306 
SelectValue(int64_t index)4307 int64_t GuidedSlackFinalizer::SelectValue(int64_t index) {
4308   const IntVar* const slack_variable = dimension_->SlackVar(index);
4309   const int64_t center = initial_values_[index];
4310   const int64_t max_delta =
4311       std::max(center - slack_variable->Min(), slack_variable->Max() - center) +
4312       1;
4313   int64_t delta = last_delta_used_[index];
4314 
4315   // The sequence of deltas is 0, 1, -1, 2, -2 ...
4316   // Only the values inside the domain of variable are returned.
4317   while (std::abs(delta) < max_delta &&
4318          !slack_variable->Contains(center + delta)) {
4319     if (delta > 0) {
4320       delta = -delta;
4321     } else {
4322       delta = -delta + 1;
4323     }
4324   }
4325   last_delta_used_.SetValue(model_->solver(), index, delta);
4326   return center + delta;
4327 }
4328 
ChooseVariable()4329 int64_t GuidedSlackFinalizer::ChooseVariable() {
4330   int64_t int_current_node = current_index_.Value();
4331   int64_t int_current_route = current_route_.Value();
4332 
4333   while (int_current_route < model_->vehicles()) {
4334     while (!model_->IsEnd(int_current_node) &&
4335            dimension_->SlackVar(int_current_node)->Bound()) {
4336       int_current_node = model_->NextVar(int_current_node)->Value();
4337     }
4338     if (!model_->IsEnd(int_current_node)) {
4339       break;
4340     }
4341     int_current_route += 1;
4342     if (int_current_route < model_->vehicles()) {
4343       int_current_node = model_->Start(int_current_route);
4344     }
4345   }
4346 
4347   CHECK(int_current_route == model_->vehicles() ||
4348         !dimension_->SlackVar(int_current_node)->Bound());
4349   current_index_.SetValue(model_->solver(), int_current_node);
4350   current_route_.SetValue(model_->solver(), int_current_route);
4351   if (int_current_route < model_->vehicles()) {
4352     return int_current_node;
4353   } else {
4354     return -1;
4355   }
4356 }
4357 }  // namespace
4358 
MakeGuidedSlackFinalizer(const RoutingDimension * dimension,std::function<int64_t (int64_t)> initializer)4359 DecisionBuilder* RoutingModel::MakeGuidedSlackFinalizer(
4360     const RoutingDimension* dimension,
4361     std::function<int64_t(int64_t)> initializer) {
4362   return solver_->RevAlloc(
4363       new GuidedSlackFinalizer(dimension, this, std::move(initializer)));
4364 }
4365 
ShortestTransitionSlack(int64_t node) const4366 int64_t RoutingDimension::ShortestTransitionSlack(int64_t node) const {
4367   CHECK_EQ(base_dimension_, this);
4368   CHECK(!model_->IsEnd(node));
4369   // Recall that the model is cumul[i+1] = cumul[i] + transit[i] + slack[i]. Our
4370   // aim is to find a value for slack[i] such that cumul[i+1] + transit[i+1] is
4371   // minimized.
4372   const int64_t next = model_->NextVar(node)->Value();
4373   if (model_->IsEnd(next)) {
4374     return SlackVar(node)->Min();
4375   }
4376   const int64_t next_next = model_->NextVar(next)->Value();
4377   const int64_t serving_vehicle = model_->VehicleVar(node)->Value();
4378   CHECK_EQ(serving_vehicle, model_->VehicleVar(next)->Value());
4379   const RoutingModel::StateDependentTransit transit_from_next =
4380       model_->StateDependentTransitCallback(
4381           state_dependent_class_evaluators_
4382               [state_dependent_vehicle_to_class_[serving_vehicle]])(next,
4383                                                                     next_next);
4384   // We have that transit[i+1] is a function of cumul[i+1].
4385   const int64_t next_cumul_min = CumulVar(next)->Min();
4386   const int64_t next_cumul_max = CumulVar(next)->Max();
4387   const int64_t optimal_next_cumul =
4388       transit_from_next.transit_plus_identity->RangeMinArgument(
4389           next_cumul_min, next_cumul_max + 1);
4390   // A few checks to make sure we're on the same page.
4391   DCHECK_LE(next_cumul_min, optimal_next_cumul);
4392   DCHECK_LE(optimal_next_cumul, next_cumul_max);
4393   // optimal_next_cumul = cumul + transit + optimal_slack, so
4394   // optimal_slack = optimal_next_cumul - cumul - transit.
4395   // In the current implementation TransitVar(i) = transit[i] + slack[i], so we
4396   // have to find the transit from the evaluators.
4397   const int64_t current_cumul = CumulVar(node)->Value();
4398   const int64_t current_state_independent_transit = model_->TransitCallback(
4399       class_evaluators_[vehicle_to_class_[serving_vehicle]])(node, next);
4400   const int64_t current_state_dependent_transit =
4401       model_
4402           ->StateDependentTransitCallback(
4403               state_dependent_class_evaluators_
4404                   [state_dependent_vehicle_to_class_[serving_vehicle]])(node,
4405                                                                         next)
4406           .transit->Query(current_cumul);
4407   const int64_t optimal_slack = optimal_next_cumul - current_cumul -
4408                                 current_state_independent_transit -
4409                                 current_state_dependent_transit;
4410   CHECK_LE(SlackVar(node)->Min(), optimal_slack);
4411   CHECK_LE(optimal_slack, SlackVar(node)->Max());
4412   return optimal_slack;
4413 }
4414 
4415 namespace {
4416 class GreedyDescentLSOperator : public LocalSearchOperator {
4417  public:
4418   explicit GreedyDescentLSOperator(std::vector<IntVar*> variables);
4419 
4420   bool MakeNextNeighbor(Assignment* delta, Assignment* deltadelta) override;
4421   void Start(const Assignment* assignment) override;
4422 
4423  private:
4424   int64_t FindMaxDistanceToDomain(const Assignment* assignment);
4425 
4426   const std::vector<IntVar*> variables_;
4427   const Assignment* center_;
4428   int64_t current_step_;
4429   // The deltas are returned in this order:
4430   // (current_step_, 0, ... 0), (-current_step_, 0, ... 0),
4431   // (0, current_step_, ... 0), (0, -current_step_, ... 0),
4432   // ...
4433   // (0, ... 0, current_step_), (0, ... 0, -current_step_).
4434   // current_direction_ keeps track what was the last returned delta.
4435   int64_t current_direction_;
4436 
4437   DISALLOW_COPY_AND_ASSIGN(GreedyDescentLSOperator);
4438 };
4439 
GreedyDescentLSOperator(std::vector<IntVar * > variables)4440 GreedyDescentLSOperator::GreedyDescentLSOperator(std::vector<IntVar*> variables)
4441     : variables_(std::move(variables)),
4442       center_(nullptr),
4443       current_step_(0),
4444       current_direction_(0) {}
4445 
MakeNextNeighbor(Assignment * delta,Assignment *)4446 bool GreedyDescentLSOperator::MakeNextNeighbor(Assignment* delta,
4447                                                Assignment* /*deltadelta*/) {
4448   static const int64_t sings[] = {1, -1};
4449   for (; 1 <= current_step_; current_step_ /= 2) {
4450     for (; current_direction_ < 2 * variables_.size();) {
4451       const int64_t variable_idx = current_direction_ / 2;
4452       IntVar* const variable = variables_[variable_idx];
4453       const int64_t sign_index = current_direction_ % 2;
4454       const int64_t sign = sings[sign_index];
4455       const int64_t offset = sign * current_step_;
4456       const int64_t new_value = center_->Value(variable) + offset;
4457       ++current_direction_;
4458       if (variable->Contains(new_value)) {
4459         delta->Add(variable);
4460         delta->SetValue(variable, new_value);
4461         return true;
4462       }
4463     }
4464     current_direction_ = 0;
4465   }
4466   return false;
4467 }
4468 
Start(const Assignment * assignment)4469 void GreedyDescentLSOperator::Start(const Assignment* assignment) {
4470   CHECK(assignment != nullptr);
4471   current_step_ = FindMaxDistanceToDomain(assignment);
4472   center_ = assignment;
4473 }
4474 
FindMaxDistanceToDomain(const Assignment * assignment)4475 int64_t GreedyDescentLSOperator::FindMaxDistanceToDomain(
4476     const Assignment* assignment) {
4477   int64_t result = std::numeric_limits<int64_t>::min();
4478   for (const IntVar* const var : variables_) {
4479     result = std::max(result, std::abs(var->Max() - assignment->Value(var)));
4480     result = std::max(result, std::abs(var->Min() - assignment->Value(var)));
4481   }
4482   return result;
4483 }
4484 }  // namespace
4485 
MakeGreedyDescentLSOperator(std::vector<IntVar * > variables)4486 std::unique_ptr<LocalSearchOperator> RoutingModel::MakeGreedyDescentLSOperator(
4487     std::vector<IntVar*> variables) {
4488   return std::unique_ptr<LocalSearchOperator>(
4489       new GreedyDescentLSOperator(std::move(variables)));
4490 }
4491 
MakeSelfDependentDimensionFinalizer(const RoutingDimension * dimension)4492 DecisionBuilder* RoutingModel::MakeSelfDependentDimensionFinalizer(
4493     const RoutingDimension* dimension) {
4494   CHECK(dimension != nullptr);
4495   CHECK(dimension->base_dimension() == dimension);
4496   std::function<int64_t(int64_t)> slack_guide = [dimension](int64_t index) {
4497     return dimension->ShortestTransitionSlack(index);
4498   };
4499   DecisionBuilder* const guided_finalizer =
4500       MakeGuidedSlackFinalizer(dimension, slack_guide);
4501   DecisionBuilder* const slacks_finalizer =
4502       solver_->MakeSolveOnce(guided_finalizer);
4503   std::vector<IntVar*> start_cumuls(vehicles_, nullptr);
4504   for (int64_t vehicle_idx = 0; vehicle_idx < vehicles_; ++vehicle_idx) {
4505     start_cumuls[vehicle_idx] = dimension->CumulVar(starts_[vehicle_idx]);
4506   }
4507   LocalSearchOperator* const hill_climber =
4508       solver_->RevAlloc(new GreedyDescentLSOperator(start_cumuls));
4509   LocalSearchPhaseParameters* const parameters =
4510       solver_->MakeLocalSearchPhaseParameters(CostVar(), hill_climber,
4511                                               slacks_finalizer);
4512   Assignment* const first_solution = solver_->MakeAssignment();
4513   first_solution->Add(start_cumuls);
4514   for (IntVar* const cumul : start_cumuls) {
4515     first_solution->SetValue(cumul, cumul->Min());
4516   }
4517   DecisionBuilder* const finalizer =
4518       solver_->MakeLocalSearchPhase(first_solution, parameters);
4519   return finalizer;
4520 }
4521 }  // namespace operations_research
4522