flight_plan.cpp

#include "flight_plan.h"

#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000

#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"




struct FlightPlan::RoutingDataModel{
    Matrix<int64_t>                 distanceMatrix;
    long                            numVehicles;
    RoutingIndexManager::NodeIndex  depot;
};

FlightPlan::FlightPlan()
{

}

bool FlightPlan::generate(double lineDistance, double minTransectLength)
{
    _waypointsENU.clear();
    _waypoints.clear();
    _arrivalPathIdx.clear();
    _returnPathIdx.clear();

#ifndef NDEBUG
    _PathVertices.clear();
#endif
#ifdef SHOW_TIME
    auto start = std::chrono::high_resolution_clock::now();
#endif
    if (!_generateTransects(lineDistance, minTransectLength))
        return false;
#ifdef SHOW_TIME
    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
    cout << endl;
   cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl;
#endif

    //=======================================
    // Route Transects using Google or-tools.
    //=======================================
    // Offset joined area.
    const BoostPolygon &jArea = _scenario.getJoineAreaENU();
    BoostPolygon jAreaOffset;
    offsetPolygon(jArea, jAreaOffset, detail::offsetConstant);

    // Create vertex list;
    BoostLineString vertices;
    size_t n_t = _transects.size()*2;
    size_t n0  = n_t+1;
    vertices.reserve(n0);
    for (auto lstring : _transects){
        for (auto vertex : lstring){
            vertices.push_back(vertex);
        }
    }
    vertices.push_back(_scenario.getHomePositonENU());

    for (long i=0; i<long(jArea.outer().size())-1; ++i) {
        vertices.push_back(jArea.outer()[i]);
    }
    for (auto ring : jArea.inners()) {
        for (auto vertex : ring)
            vertices.push_back(vertex);
    }
    size_t n1 = vertices.size();
    // Generate routing model.
    RoutingDataModel dataModel;
    Matrix<double> connectionGraph(n1, n1);

#ifdef SHOW_TIME
    start = std::chrono::high_resolution_clock::now();
#endif
    _generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
#ifdef SHOW_TIME
    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 transit_callback_index = 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(transit_callback_index);


    // Define Constraints.
    size_t n = _transects.size()*2;
    Solver *solver = routing.solver();
    for (size_t i=0; i<n; i=i+2){
//        auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
//        auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
//        auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
//        auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
//        auto c = solver->MakeNonEquality(cond0, cond1);
//        solver->AddConstraint(c);

        // alternative
        auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
        auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
        auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
        auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
        vector<IntVar*> conds{cond0, cond1};
        auto c = solver->MakeAllDifferent(conds);
        solver->MakeRejectFilter();
        solver->AddConstraint(c);
    }


    // 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 SHOW_TIME
    start = std::chrono::high_resolution_clock::now();
#endif
    const Assignment* solution = routing.SolveWithParameters(searchParameters);
#ifdef 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 waypoints from solution.
    long index = routing.Start(0);
    std::vector<size_t> route;
    route.push_back(manager.IndexToNode(index).value());
    while (!routing.IsEnd(index)){
        index = solution->Value(routing.NextVar(index));
        route.push_back(manager.IndexToNode(index).value());
    }

    // Connect transects
#ifndef NDEBUG
    _PathVertices = vertices;
#endif

    {
    _waypointsENU.push_back(vertices[route[0]]);
    vector<size_t> pathIdx;
    long arrivalPathLength = 0;
    for (long i=0; i<long(route.size())-1; ++i){
        size_t idx0 = route[i];
        size_t idx1 = route[i+1];
        pathIdx.clear();
        shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
        if ( i==0 )
            arrivalPathLength = pathIdx.size();
        for (size_t j=1; j<pathIdx.size(); ++j)
            _waypointsENU.push_back(vertices[pathIdx[j]]);
    }


    long returnPathLength = pathIdx.size();
    for (long i=returnPathLength; i > 0; --i)
        _returnPathIdx.push_back(_waypointsENU.size()-i);

    for (long i=0; i < arrivalPathLength; ++i)
        _arrivalPathIdx.push_back(i);
    }

    // Back transform waypoints.
    GeoPoint3D origin{_scenario.getOrigin()};
    for (auto vertex : _waypointsENU) {
        GeoPoint3D geoVertex;
        fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
        _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
    }

    return true;
}

bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength)
{
    _transects.clear();
    if (_scenario.getTilesENU().size() != _progress.size()){
        ostringstream strstream;
        strstream << "Number of tiles ("
                  << _scenario.getTilesENU().size()
                  << ") is not equal to progress array length ("
                  << _progress.size()
                  << ")";
        errorString = strstream.str();
        return false;
    }

    // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
    size_t num_tiles = _progress.size();
    vector<BoostPolygon> processedTiles;
    {
        const auto &tiles = _scenario.getTilesENU();
        for (size_t i=0; i<num_tiles; ++i) {
            if (_progress[i] == 100){
                processedTiles.push_back(tiles[i]);
            }
        }

        if (processedTiles.size() == num_tiles)
            return true;
    }

    // Convert measurement area and tiles to clipper path.
    ClipperLib::Path mAreaClipper;
    for ( auto vertex : _scenario.getMeasurementAreaENU().outer() ){
        mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
                                                    static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
    }
    vector<ClipperLib::Path> processedTilesClipper;
    for (auto t : processedTiles){
        ClipperLib::Path path;
        for (auto vertex : t.outer()){
            path.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
                                                static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
        }
        processedTilesClipper.push_back(path);
    }

    const min_bbox_rt &bbox     = _scenario.getMeasurementAreaBBoxENU();
    double alpha                = bbox.angle;
    double x0                   = bbox.corners.outer()[0].get<0>();
    double y0                   = bbox.corners.outer()[0].get<1>();
    double bboxWidth            = bbox.width;
    double bboxHeight           = bbox.height;
    double delta                = detail::offsetConstant;

    size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects
    vector<double> yCoords;
    yCoords.reserve(num_t);
    double y = -delta;
    for (size_t i=0; i < num_t; ++i) {
        yCoords.push_back(y);
        y += lineDistance;
    }


    // Generate transects and convert them to clipper path.
    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-alpha*180/M_PI);
    trans::translate_transformer<double, 2, 2> translate_back(x0, y0);
    vector<ClipperLib::Path> transectsClipper;
    transectsClipper.reserve(num_t);
    for (size_t i=0; i < num_t; ++i) {
        // calculate transect
        BoostPoint v1{-delta, yCoords[i]};
        BoostPoint v2{bboxWidth+delta, yCoords[i]};
        BoostLineString transect;
        transect.push_back(v1);
        transect.push_back(v2);

        // transform back
        BoostLineString temp_transect;
        bg::transform(transect, temp_transect, rotate_back);
        transect.clear();
        bg::transform(temp_transect, transect, translate_back);


        ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(transect[0].get<0>()*CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(transect[0].get<1>()*CLIPPER_SCALE)};
        ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(transect[1].get<0>()*CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(transect[1].get<1>()*CLIPPER_SCALE)};
        ClipperLib::Path path{c1, c2};
        transectsClipper.push_back(path);
    }

    // Perform clipping.
    // Clip transects to measurement area.
    ClipperLib::Clipper clipper;
    clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
    clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
    ClipperLib::PolyTree clippedTransecsPolyTree1;
    clipper.Execute(ClipperLib::ctIntersection, clippedTransecsPolyTree1, ClipperLib::pftNonZero, ClipperLib::pftNonZero);

    // Subtract holes (tiles with measurement_progress == 100) from transects.
    clipper.Clear();
    for (auto child : clippedTransecsPolyTree1.Childs)
        clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
    clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
    ClipperLib::PolyTree clippedTransecsPolyTree2;
    clipper.Execute(ClipperLib::ctDifference, clippedTransecsPolyTree2, ClipperLib::pftNonZero, ClipperLib::pftNonZero);

    // Extract transects from  PolyTree and convert them to BoostLineString
    for (auto child : clippedTransecsPolyTree2.Childs){
        ClipperLib::Path clipperTransect = child->Contour;
        BoostPoint v1{static_cast<double>(clipperTransect[0].X)/CLIPPER_SCALE,
                      static_cast<double>(clipperTransect[0].Y)/CLIPPER_SCALE};
        BoostPoint v2{static_cast<double>(clipperTransect[1].X)/CLIPPER_SCALE,
                      static_cast<double>(clipperTransect[1].Y)/CLIPPER_SCALE};

        BoostLineString transect{v1, v2};
        if (bg::length(transect) >= minTransectLength)
            _transects.push_back(transect);

    }

    if (_transects.size() < 1)
        return false;

    return true;
}

void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
                                       const BoostPolygon &polygonOffset,
                                       size_t n0,
                                       FlightPlan::RoutingDataModel &dataModel,
                                       Matrix<double> &graph)
{
#ifdef SHOW_TIME
    auto start = std::chrono::high_resolution_clock::now();
#endif
    graphFromPolygon(polygonOffset, vertices, graph);
#ifdef SHOW_TIME
    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
    cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
#endif
//    cout << endl;
//    for (size_t i=0; i<graph.size(); ++i){
//        vector<double> &row = graph[i];
//        for (size_t j=0; j<row.size();++j){
//            cout << "(" << i << "," << j << "):" << row[j] << " ";
//        }
//        cout << endl;
//    }
//    cout << endl;
    Matrix<double> distanceMatrix(graph);
#ifdef SHOW_TIME
    start = std::chrono::high_resolution_clock::now();
#endif
    toDistanceMatrix(distanceMatrix);
#ifdef SHOW_TIME
    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
    cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;
#endif

    dataModel.distanceMatrix.setDimension(n0, n0);
    for (size_t i=0; i<n0; ++i){
        dataModel.distanceMatrix.set(i, i, 0);
        for (size_t j=i+1; j<n0; ++j){
            dataModel.distanceMatrix.set(i, j, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
            dataModel.distanceMatrix.set(j, i, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
        }
    }

    dataModel.numVehicles   = 1;
    dataModel.depot         = n0-1;
}