route.cpp

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<std::size_t>(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<TransectInfo> 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<double> 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::milliseconds>(
      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::milliseconds>(
      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<size_t> 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<size_t> &idxArray,
                                std::vector<BoostPoint> &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<size_t> idxList;
      shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
      if (i != route_idx.size() - 2) {
        idxList.pop_back();
      }
      idx2Vertex(idxList, route);
    }
  }
  return true;
}