bool flight_plan::route(const BoostPolygon &area, const flight_plan::Transects &transects, Transects &transectsRouted, flight_plan::Route &route, string &errorString) { //======================================= // Route Transects using Google or-tools. //======================================= // Create vertex list; BoostLineString vertices; size_t n0 = 0; for (const auto &t : transects) { n0 += std::min(t.size(), 2); } vertices.reserve(n0); struct TransectInfo { TransectInfo(size_t n, bool f) : index(n), front(f) {} size_t index; bool front; }; std::vector transectInfoList; for (size_t i = 0; i < transects.size(); ++i) { const auto &t = transects[i]; vertices.push_back(t.front()); transectInfoList.push_back(TransectInfo{i, true}); if (t.size() >= 2) { vertices.push_back(t.back()); transectInfoList.push_back(TransectInfo{i, false}); } } for (long i = 0; i < long(area.outer().size()) - 1; ++i) { vertices.push_back(area.outer()[i]); } for (auto &ring : area.inners()) { for (auto &vertex : ring) vertices.push_back(vertex); } size_t n1 = vertices.size(); // Generate routing model. RoutingDataModel dataModel; Matrix connectionGraph(n1, n1); // Offset joined area. BoostPolygon areaOffset; offsetPolygon(area, areaOffset, detail::offsetConstant); #ifdef SNAKE_SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); #endif generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph); #ifdef SNAKE_SHOW_TIME auto delta = std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start); cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl; #endif // Create Routing Index Manager. RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles, dataModel.depot); // Create Routing Model. RoutingModel routing(manager); // Create and register a transit callback. const int transitCallbackIndex = routing.RegisterTransitCallback( [&dataModel, &manager](int64 from_index, int64 to_index) -> int64 { // Convert from routing variable Index to distance matrix NodeIndex. auto from_node = manager.IndexToNode(from_index).value(); auto to_node = manager.IndexToNode(to_index).value(); return dataModel.distanceMatrix.get(from_node, to_node); }); // Define cost of each arc. routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex); // Define Constraints (this constraints have a huge impact on the // solving time, improvments could be done, e.g. SearchFilter). #ifdef SNAKE_DEBUG std::cout << "snake::route(): Constraints:" << std::endl; #endif Solver *solver = routing.solver(); size_t index = 0; for (size_t i = 0; i < transects.size(); ++i) { const auto &t = transects[i]; #ifdef SNAKE_DEBUG std::cout << "i = " << i << ": t.size() = " << t.size() << std::endl; #endif if (t.size() >= 2) { auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index)); auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index + 1)); auto cond0 = routing.NextVar(idx0)->IsEqual(idx1); auto cond1 = routing.NextVar(idx1)->IsEqual(idx0); auto c = solver->MakeNonEquality(cond0, cond1); solver->AddConstraint(c); #ifdef SNAKE_DEBUG std::cout << "( next(" << index << ") == " << index + 1 << " ) != ( next(" << index + 1 << ") == " << index << " )" << std::endl; #endif index += 2; } else { index += 1; } } // Setting first solution heuristic. RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters(); searchParameters.set_first_solution_strategy( FirstSolutionStrategy::PATH_CHEAPEST_ARC); google::protobuf::Duration *tMax = new google::protobuf::Duration(); // seconds tMax->set_seconds(10); searchParameters.set_allocated_time_limit(tMax); // Solve the problem. #ifdef SNAKE_SHOW_TIME start = std::chrono::high_resolution_clock::now(); #endif const Assignment *solution = routing.SolveWithParameters(searchParameters); #ifdef SNAKE_SHOW_TIME delta = std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start); cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl; #endif if (!solution || solution->Size() <= 1) { errorString = "Not able to solve the routing problem."; return false; } // Extract index list from solution. index = routing.Start(0); std::vector route_idx; route_idx.push_back(manager.IndexToNode(index).value()); while (!routing.IsEnd(index)) { index = solution->Value(routing.NextVar(index)); route_idx.push_back(manager.IndexToNode(index).value()); } // Helper Lambda. auto idx2Vertex = [&vertices](const std::vector &idxArray, std::vector &path) { for (auto idx : idxArray) path.push_back(vertices[idx]); }; // Construct route. for (size_t i = 0; i < route_idx.size() - 1; ++i) { size_t idx0 = route_idx[i]; size_t idx1 = route_idx[i + 1]; const auto &info1 = transectInfoList[idx0]; const auto &info2 = transectInfoList[idx1]; if (info1.index == info2.index) { // same transect? if (!info1.front) { // transect reversal needed? BoostLineString reversedTransect; const auto &t = transects[info1.index]; for (auto it = t.end() - 1; it >= t.begin(); --it) { reversedTransect.push_back(*it); } transectsRouted.push_back(reversedTransect); for (auto it = reversedTransect.begin(); it < reversedTransect.end() - 1; ++it) { route.push_back(*it); } } else { const auto &t = transects[info1.index]; for (auto it = t.begin(); it < t.end() - 1; ++it) { route.push_back(*it); } transectsRouted.push_back(t); } } else { std::vector idxList; shortestPathFromGraph(connectionGraph, idx0, idx1, idxList); if (i != route_idx.size() - 2) { idxList.pop_back(); } idx2Vertex(idxList, route); } } return true; }