snake.cpp 23.3 KB
Newer Older
Valentin Platzgummer's avatar
Valentin Platzgummer committed
1 2 3 4 5 6
#include <iostream>

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

7 8 9 10
#ifndef NDEBUG
//#define SHOW_TIME
#endif

Valentin Platzgummer's avatar
Valentin Platzgummer committed
11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77
using namespace snake_geometry;
using namespace std;

namespace bg = boost::geometry;
namespace trans = bg::strategy::transform;

namespace snake {

Scenario::Scenario() :
    _mAreaBoundingBox(min_bbox_rt{0, 0, 0, BoostPolygon{}})
{

}

bool Scenario::addArea(Area &area)
    {
        if (area.geoPolygon.size() < 3){
            errorString = "Area has less than three vertices.";
            return false;
        }
        if (area.type == MeasurementArea)
            return Scenario::_setMeasurementArea(area);
        else if (area.type == ServiceArea)
            return Scenario::_setServiceArea(area);
        else if (area.type == Corridor)
            return Scenario::_setCorridor(area);
        return false;
}

bool Scenario::defined(double tileWidth, double tileHeight, double minTileArea)
{
    if (!_areas2enu())
        return false;
    if (!_calculateBoundingBox())
        return false;
    if (!_calculateTiles(tileWidth, tileHeight, minTileArea))
        return false;
    if (!_calculateJoinedArea())
        return false;

    return true;
}

bool Scenario::_areas2enu()
{
    if (_measurementArea.geoPolygon.size() > 0){
        _measurementAreaENU.clear();
        for(auto vertex : _measurementArea.geoPolygon) {
                Point3D ENUVertex;
                toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _measurementArea.altitude}, ENUVertex);
                _measurementAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
        }
        bg::correct(_measurementAreaENU);

        _serviceAreaENU.clear();
        if (_serviceArea.geoPolygon.size() > 0){
            for(auto vertex : _serviceArea.geoPolygon) {
                    Point3D ENUVertex;
                    toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _serviceArea.altitude}, ENUVertex);
                    _serviceAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
            }
        } else{
            errorString = "Service area has no vertices.";
            return false;
        }
        bg::correct(_serviceAreaENU);
        polygonCenter(_serviceAreaENU, _homePositionENU);
78
        fromENU(_geoOrigin, Point3D{_homePositionENU.get<0>(), _homePositionENU.get<1>(), 0}, _homePosition);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153

        _corridorENU.clear();
        if (_corridor.geoPolygon.size() > 0){
            for(auto vertex : _corridor.geoPolygon) {
                    Point3D ENUVertex;
                    toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _corridor.altitude}, ENUVertex);
                    _corridorENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
            }
        }
        bg::correct(_corridorENU);

        return true;
    }

    errorString = "Measurement area has no vertices.";
    return false;
}

bool Scenario::_setMeasurementArea(Area &area)
{
    if (area.geoPolygon.size() <= 0)
        return false;
    GeoPoint2D origin2D = area.geoPolygon[0];
    _geoOrigin = GeoPoint3D{origin2D[0], origin2D[1], 0};
    _measurementArea = area;
    _measurementAreaENU.clear();
    _serviceAreaENU.clear();
    _corridorENU.clear();
    return true;

}

bool Scenario::_setServiceArea(Area &area)
{
    if (area.geoPolygon.size() <= 0)
        return false;
    _serviceArea = area;
    _serviceAreaENU.clear();
    return true;
}

bool Scenario::_setCorridor(Area &area)
{
    if (area.geoPolygon.size() <= 0)
        return false;
    _corridor = area;
    _corridorENU.clear();
    return true;
}

bool Scenario::_calculateBoundingBox()
{
    minimalBoundingBox(_measurementAreaENU, _mAreaBoundingBox);
    return true;
}


/**
 * Devides the (measurement area) bounding  box into tiles and clips it to the measurement area.
 *
 * Devides the (measurement area) bounding  box into tiles of width \p tileWidth and height \p tileHeight.
 * Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea.
 * The function assumes that \a _measurementAreaENU and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref
 * Scenario::_calculateBoundingBox().
 *
 * @param tileWidth The width (>0) of a tile.
 * @param tileHeight The heigth (>0) of a tile.
 * @param minTileArea The minimal area (>0) of a tile.
 *
 * @return Returns true if successful.
 */
bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTileArea)
{
    _tilesENU.clear();
    _tileCenterPointsENU.clear();
154 155
    _tiles.clear();
    _tileCenterPoints.clear();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232

    if (tileWidth <= 0 || tileHeight <= 0 || minTileArea <= 0) {
        errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive.";
        return false;
    }

    double bbox_width = _mAreaBoundingBox.width;
    double bbox_height = _mAreaBoundingBox.height;

    BoostPoint origin = _mAreaBoundingBox.corners.outer()[0];

    //cout << "Origin: " << origin[0] << " " << origin[1] << endl;
    // Transform _measurementAreaENU polygon to bounding box coordinate system.
    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(_mAreaBoundingBox.angle*180/M_PI);
    trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(), -origin.get<1>());
    BoostPolygon translated_polygon;
    BoostPolygon rotated_polygon;
    boost::geometry::transform(_measurementAreaENU, translated_polygon, translate);
    boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
    bg::correct(rotated_polygon);

    //cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;

    size_t i_max = ceil(bbox_width/tileWidth);
    size_t j_max = ceil(bbox_height/tileHeight);

    if (i_max < 1 || j_max < 1) {
        errorString = "tileWidth or tileHeight to small.";
        return false;
    }


    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-_mAreaBoundingBox.angle*180/M_PI);
    trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(), origin.get<1>());
    for (size_t i = 0; i < i_max; ++i){
        double x_min = tileWidth*i;
        double x_max = x_min + tileWidth;
        for (size_t j = 0; j < j_max; ++j){
            double y_min = tileHeight*j;
            double y_max = y_min + tileHeight;

            BoostPolygon tile_unclipped;
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_max});
            tile_unclipped.outer().push_back(BoostPoint{x_max, y_max});
            tile_unclipped.outer().push_back(BoostPoint{x_max, y_min});
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});


            std::deque<BoostPolygon> boost_tiles;
            if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, boost_tiles))
                continue;

           for (BoostPolygon t : boost_tiles)
           {
                if (bg::area(t) > minTileArea){
                    // Transform boost_tile to world coordinate system.
                    BoostPolygon rotated_tile;
                    BoostPolygon translated_tile;
                    boost::geometry::transform(t, rotated_tile, rotate_back);
                    boost::geometry::transform(rotated_tile, translated_tile, translate_back);

                    // Store tile and calculate center point.
                    _tilesENU.push_back(translated_tile);
                    BoostPoint tile_center;
                    polygonCenter(translated_tile, tile_center);
                    _tileCenterPointsENU.push_back(tile_center);
                }
           }
        }
    }

    if (_tilesENU.size() < 1){
        errorString = "No tiles calculated. Is the minTileArea parameter large enough?";
        return false;
    }

233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249
    for ( auto tile : _tilesENU){
        GeoPoint3DList geoTile;
        for ( int i=0; i < int(tile.outer().size())-1; ++i) {
            BoostPoint vertex(tile.outer()[i]);
            GeoPoint3D geoVertex;
            fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
            geoTile.push_back(geoVertex);
        }
        _tiles.push_back(geoTile);
    }

    for ( auto vertex : _tileCenterPointsENU){
        GeoPoint3D geoVertex;
        fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
        _tileCenterPoints.push_back(geoVertex);
    }

Valentin Platzgummer's avatar
Valentin Platzgummer committed
250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266
    return true;
}

bool Scenario::_calculateJoinedArea()
{
    _joinedAreaENU.clear();
    // Measurement area and service area overlapping?
    bool overlapingSerMeas = bg::intersects(_measurementAreaENU, _serviceAreaENU) ? true : false;
    bool corridorValid = _corridorENU.outer().size() > 0 ? true : false;


    // Check if corridor is connecting measurement area and service area.
    bool corridor_is_connection = false;
    if (corridorValid) {
        // Corridor overlaping with measurement area?
        if ( bg::intersects(_corridorENU, _measurementAreaENU) ) {
            // Corridor overlaping with service area?
267
            if ( bg::intersects(_corridorENU, _serviceAreaENU) ) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
268
                corridor_is_connection = true;
269
            }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318
        }
    }

    // Are areas joinable?
    std::deque<BoostPolygon> sol;
    BoostPolygon partialArea = _measurementAreaENU;
    if (overlapingSerMeas){
        if(corridor_is_connection){
            bg::union_(partialArea, _corridorENU, sol);
        }
    } else if (corridor_is_connection){
        bg::union_(partialArea, _corridorENU, sol);
    } else {
        errorString = "Areas are not overlapping";
        return false;
    }

    if (sol.size() > 0) {
        partialArea = sol[0];
        sol.clear();
    }

    // Join areas.
    bg::union_(partialArea, _serviceAreaENU, sol);

    if (sol.size() > 0) {
        _joinedAreaENU = sol[0];
    } else {
        return false;
    }

    return true;
}

FlightPlan::FlightPlan()
{

}

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

#ifndef NDEBUG
    _PathVertices.clear();
#endif
319
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
320
    auto start = std::chrono::high_resolution_clock::now();
321
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
322 323
    if (!_generateTransects(lineDistance, minTransectLength))
        return false;
324
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
325 326 327
    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;
328
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361

    //=======================================
    // 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_t dataModel;
    Matrix<double> connectionGraph(n1, n1);

362
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
363
    start = std::chrono::high_resolution_clock::now();
364
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
365
    _generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
366
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
367 368
    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
    cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl;
369
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385

    // 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);
        });

386 387 388
    // Define cost of each arc.
    routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);

Valentin Platzgummer's avatar
Valentin Platzgummer committed
389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421

    // 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.
422
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
423
    start = std::chrono::high_resolution_clock::now();
424
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
425
    const Assignment* solution = routing.SolveWithParameters(searchParameters);
426
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
427 428
    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
    cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl;
429
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452

    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;
453
    long arrivalPathLength = 0;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
454 455 456 457 458
    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);
459 460
        if ( i==0 )
            arrivalPathLength = pathIdx.size();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
461 462 463
        for (size_t j=1; j<pathIdx.size(); ++j)
            _waypointsENU.push_back(vertices[pathIdx[j]]);
    }
464 465 466 467 468 469 470 471


    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);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488
    }

    // 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()){
489 490 491 492 493 494 495
        ostringstream strstream;
        strstream << "Number of tiles ("
                  << _scenario.getTilesENU().size()
                  << ") is not equal to progress array length ("
                  << _progress.size()
                  << ")";
        errorString = strstream.str();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617
        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_t &dataModel,
                                       Matrix<double> &graph)
{
618
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
619
    auto start = std::chrono::high_resolution_clock::now();
620
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
621
    graphFromPolygon(polygonOffset, vertices, graph);
622
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
623 624
    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
    cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
625
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
626 627 628 629 630 631 632 633 634 635
//    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);
636
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
637
    start = std::chrono::high_resolution_clock::now();
638
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
639
    toDistanceMatrix(distanceMatrix);
640
#ifdef SHOW_TIME
Valentin Platzgummer's avatar
Valentin Platzgummer committed
641 642
    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
    cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;
643
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
644 645 646 647 648 649 650 651 652 653 654 655 656 657

    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;
}

658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676
Area::Area() : Area(GeoPoint2DList(), 0, 1, AreaType::MeasurementArea)
{

}

Area::Area(const GeoPoint2DList &gP, AreaType tp) : Area(gP, 0, 1, tp)
{

}

Area::Area(const GeoPoint2DList &gP, double alt, size_t l, AreaType tp) :
    geoPolygon(gP)
  , altitude(alt)
  , layers(l)
  , type(tp)
{

}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
677
}