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