SnakeThread.cc 17.7 KB
Newer Older
Valentin Platzgummer's avatar
Valentin Platzgummer committed
1 2 3 4 5 6 7 8 9 10 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
#include "SnakeThread.h"

#include <QGeoCoordinate>
#include <QMutexLocker>

#include "QGCApplication.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"

#include <iostream>
#include <memory>
#include <mutex>
#include <shared_mutex>

#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "snake.h"

template <class Callable> class CommandRAII {
public:
  CommandRAII(Callable &fun) : _fun(fun) {}

  ~CommandRAII() { _fun(); }

private:
  Callable _fun;
};

using QVariantList = QList<QVariant>;

using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;

class SnakeThread::Impl {
public:
  struct Input {

    Input()
        : tileWidth(5 * si::meter), tileHeight(5 * si::meter),
42
          minTileArea(5 * si::meter * si::meter), lineDistance(2 * si::meter),
Valentin Platzgummer's avatar
Valentin Platzgummer committed
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 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
          minTransectLength(1 * si::meter), scenarioChanged(true),
          progressChanged(true), routeParametersChanged(true) {}

    QList<QGeoCoordinate> mArea;
    QGeoCoordinate ENUOrigin;
    QList<QGeoCoordinate> sArea;
    QList<QGeoCoordinate> corridor;
    Length tileWidth;
    Length tileHeight;
    Area minTileArea;

    Length lineDistance;
    Length minTransectLength;

    QNemoProgress progress;

    std::atomic_bool scenarioChanged;
    std::atomic_bool progressChanged;
    std::atomic_bool routeParametersChanged;

    mutable std::shared_timed_mutex mutex;
  };

  struct Output {
    Output()
        : calcInProgress(false), tilesUpdated(false), waypointsUpdated(false),
          progressUpdated(false) {}

    SnakeTiles tiles;
    QVariantList tileCenterPoints;
    SnakeTilesLocal tilesENU;
    QVector<QPointF> tileCenterPointsENU;
    QGeoCoordinate ENUOrigin;

    QVector<QGeoCoordinate> waypoints;
    QVector<QGeoCoordinate> arrivalPath;
    QVector<QGeoCoordinate> returnPath;
    QVector<QPointF> waypointsENU;
    QVector<QPointF> arrivalPathENU;
    QVector<QPointF> returnPathENU;

    QString errorMessage;

    std::atomic_bool calcInProgress;
    bool tilesUpdated;
    bool waypointsUpdated;
    bool progressUpdated;

    mutable std::shared_timed_mutex mutex;
  };

  Impl(SnakeThread *p);

  bool precondition() const;
  bool doTopicServiceSetup();

  // Internal data.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
100 101 102 103 104 105
  snake::BoostPolygon mAreaENU;
  snake::BoostPolygon sAreaENU;
  snake::BoostPolygon corridorENU;
  snake::BoostPolygon jAreaENU;
  std::vector<snake::BoostPolygon> tilesENU;
  QGeoCoordinate ENUOrigin;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
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 154 155 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
  SnakeThread *parent;

  // Input
  Input input;
  // Output
  Output output;
};

SnakeThread::Impl::Impl(SnakeThread *p) : parent(p) {}

SnakeThread::SnakeThread(QObject *parent)
    : QThread(parent), pImpl(std::make_unique<Impl>(this)) {}

SnakeThread::~SnakeThread() {}

void SnakeThread::setMeasurementArea(
    const QList<QGeoCoordinate> &measurementArea) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.mArea = measurementArea;
  for (auto &vertex : this->pImpl->input.mArea) {
    vertex.setAltitude(0);
  }
}

void SnakeThread::setServiceArea(const QList<QGeoCoordinate> &serviceArea) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.sArea = serviceArea;
  for (auto &vertex : this->pImpl->input.sArea) {
    vertex.setAltitude(0);
  }
}

void SnakeThread::setCorridor(const QList<QGeoCoordinate> &corridor) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.corridor = corridor;
  for (auto &vertex : this->pImpl->input.corridor) {
    vertex.setAltitude(0);
  }
}

void SnakeThread::setProgress(const QVector<int> &progress) {
  this->pImpl->input.progressChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.progress.progress() = progress;
}

SnakeTiles SnakeThread::tiles() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tiles;
}

SnakeTilesLocal SnakeThread::tilesENU() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tilesENU;
}

QGeoCoordinate SnakeThread::ENUOrigin() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.ENUOrigin;
}

QVariantList SnakeThread::tileCenterPoints() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tileCenterPoints;
}

QVector<int> SnakeThread::progress() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->input.progress.progress();
}

bool SnakeThread::calcInProgress() const {
  return this->pImpl->output.calcInProgress.load();
}

QString SnakeThread::errorMessage() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.errorMessage;
}

bool SnakeThread::tilesUpdated() {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tilesUpdated;
}

bool SnakeThread::waypointsUpdated() {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.waypointsUpdated;
}

bool SnakeThread::progressUpdated() {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.progressUpdated;
}

QVector<QGeoCoordinate> SnakeThread::waypoints() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.waypoints;
}

QVector<QGeoCoordinate> SnakeThread::arrivalPath() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.arrivalPath;
}

QVector<QGeoCoordinate> SnakeThread::returnPath() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.returnPath;
}

Length SnakeThread::lineDistance() const {
  SharedLock lk(this->pImpl->input.mutex);
  return this->pImpl->input.lineDistance;
}

void SnakeThread::setLineDistance(Length lineDistance) {
  this->pImpl->input.routeParametersChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.routeParametersChanged = true;
  this->pImpl->input.lineDistance = lineDistance;
}

Area SnakeThread::minTileArea() const {
  SharedLock lk(this->pImpl->input.mutex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
233
  return this->pImpl->input.minTileArea;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
234 235 236 237 238
}

void SnakeThread::setMinTileArea(Area minTileArea) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
239
  this->pImpl->input.minTileArea = minTileArea;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
240 241 242 243
}

Length SnakeThread::tileHeight() const {
  SharedLock lk(this->pImpl->input.mutex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
244
  return this->pImpl->input.tileHeight;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
245 246 247 248 249
}

void SnakeThread::setTileHeight(Length tileHeight) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
250
  this->pImpl->input.tileHeight = tileHeight;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
251 252 253 254
}

Length SnakeThread::tileWidth() const {
  SharedLock lk(this->pImpl->input.mutex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
255
  return this->pImpl->input.tileWidth;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
256 257 258 259 260
}

void SnakeThread::setTileWidth(Length tileWidth) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
261
  this->pImpl->input.tileWidth = tileWidth;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285
}

void SnakeThread::run() {
#ifndef NDEBUG
  auto startTime = std::chrono::high_resolution_clock::now();
#endif
  // Signal that calculation is in progress.
  this->pImpl->output.calcInProgress.store(true);
  emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
#ifndef NDEBUG
  auto onExit = [this, &startTime] {
#else
  auto onExit = [this] {
#endif
#ifndef NDEBUG
    qDebug() << "SnakeThread::run() execution time: "
             << std::chrono::duration_cast<std::chrono::milliseconds>(
                    std::chrono::high_resolution_clock::now() - startTime)
                    .count()
             << " ms";
#endif
    this->pImpl->output.calcInProgress.store(false);
    emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
  };
Valentin Platzgummer's avatar
Valentin Platzgummer committed
286
  CommandRAII<decltype(onExit)> commandRAII(onExit);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
287 288

  bool tilesValid = true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
289 290 291 292
  bool progressValid = false;
  snake::Length lineDistance;
  snake::Length minTransectLength;
  std::vector<int> progress;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
293 294 295 296 297 298 299 300 301 302 303 304 305 306
  {
    // Check preconditions.
    SharedLock lk(this->pImpl->input.mutex);
    if (this->pImpl->input.mArea.size() < 3) {
      UniqueLock uLock(this->pImpl->output.mutex);
      this->pImpl->output.errorMessage = "Measurement area invalid: size < 3.";
      tilesValid = false;
    } else if (this->pImpl->input.sArea.size() < 3) {
      UniqueLock uLock(this->pImpl->output.mutex);
      this->pImpl->output.errorMessage = "Service area invalid: size < 3.";
      tilesValid = false;
    } else {
      // Update Scenario if necessary
      if (this->pImpl->input.scenarioChanged) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
307 308
        // Set Origin
        this->pImpl->ENUOrigin = this->pImpl->input.mArea.front();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
309
        // Update measurement area.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
310 311 312
        this->pImpl->mAreaENU.clear();
        snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.mArea,
                         this->pImpl->mAreaENU);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
313
        // Update service area.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
314 315 316
        this->pImpl->sAreaENU.clear();
        snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.sArea,
                         this->pImpl->sAreaENU);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
317
        // Update corridor.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337
        this->pImpl->corridorENU.clear();
        snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.corridor,
                         this->pImpl->corridorENU);
        // Fetch parametes.
        auto tileHeight = this->pImpl->input.tileHeight;
        auto tileWidth = this->pImpl->input.tileWidth;
        auto minTileArea = this->pImpl->input.minTileArea;
        // Update tiles.
        this->pImpl->tilesENU.clear();
        this->pImpl->jAreaENU.clear();
        std::string errorString;
        snake::BoundingBox bbox;
        if (!snake::joinedArea(this->pImpl->mAreaENU, this->pImpl->sAreaENU,
                               this->pImpl->corridorENU, this->pImpl->jAreaENU,
                               errorString) ||
            !snake::tiles(this->pImpl->mAreaENU, tileHeight, tileWidth,
                          minTileArea, this->pImpl->tilesENU, bbox,
                          errorString)) {
          UniqueLock uLock(this->pImpl->output.mutex);
          this->pImpl->output.errorMessage = errorString.c_str();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
338 339 340
          tilesValid = false;
        }
      }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357
      if (tilesValid) {
        // Sample data
        lineDistance = this->pImpl->input.lineDistance;
        minTransectLength = this->pImpl->input.minTransectLength;
        // Verify progress.
        size_t nTiles = this->pImpl->tilesENU.size();
        if (size_t(this->pImpl->input.progress.progress().size()) != nTiles) {
          for (size_t i = 0; i < nTiles; ++i) {
            progress.push_back(0);
          }
        } else {
          for (size_t i = 0; i < nTiles; ++i) {
            progress.push_back(this->pImpl->input.progress.progress()[i]);
          }
        }
        progressValid = tilesValid;
      }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
358 359 360
    }
  }

Valentin Platzgummer's avatar
Valentin Platzgummer committed
361 362
  bool waypointsValid = true;
  snake::Transects transects;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
363
  std::vector<snake::TransectInfo> transectsInfo;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
364
  snake::Route route;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
365 366 367 368 369
  bool needWaypointUpdate = this->pImpl->input.scenarioChanged ||
                            this->pImpl->input.routeParametersChanged ||
                            this->pImpl->input.progressChanged;
  if (tilesValid) {
    if (needWaypointUpdate) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
370
      // Data needed for transects.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
371
      std::string errorString;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
372 373 374 375 376 377 378 379 380 381
      snake::BoundingBox bbox;
      snake::minimalBoundingBox(this->pImpl->mAreaENU, bbox);
      snake::Angle alpha(bbox.angle * si::radian);
      snake::BoostPoint home;
      snake::polygonCenter(this->pImpl->sAreaENU, home);
      transects.push_back(bg::model::linestring<snake::BoostPoint>{home});
      // Create transects.
      bool success = snake::transectsFromScenario(
          lineDistance, minTransectLength, alpha, this->pImpl->mAreaENU,
          this->pImpl->tilesENU, progress, transects, errorString);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
382 383 384 385 386
      if (!success) {
        UniqueLock lk(this->pImpl->output.mutex);
        this->pImpl->output.errorMessage = errorString.c_str();
        waypointsValid = false;
      } else if (transects.size() > 1) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
387
        // Route transects.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
388 389
        success = snake::route(this->pImpl->jAreaENU, transects, transectsInfo,
                               route, errorString);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
390 391 392 393 394 395 396 397 398 399
        if (!success) {
          UniqueLock lk(this->pImpl->output.mutex);
          this->pImpl->output.errorMessage = errorString.c_str();
          waypointsValid = false;
        }
      } else {
        // No transects
        waypointsValid = false;
      }
    }
400 401
  } else {
    waypointsValid = false;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422
  }

  UniqueLock lk(this->pImpl->output.mutex);
  // Store tiles.
  this->pImpl->output.tilesUpdated = false;
  if (!tilesValid) {
    // Clear some output data.
    this->pImpl->output.tiles.polygons().clear();
    this->pImpl->output.tileCenterPoints.clear();
    this->pImpl->output.tilesENU.polygons().clear();
    this->pImpl->output.tileCenterPointsENU.clear();
    this->pImpl->output.ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
    this->pImpl->output.tilesUpdated = true;
  } else if (this->pImpl->input.scenarioChanged) {
    this->pImpl->input.scenarioChanged = false;
    // Clear some output data.
    this->pImpl->output.tiles.polygons().clear();
    this->pImpl->output.tileCenterPoints.clear();
    this->pImpl->output.tilesENU.polygons().clear();
    this->pImpl->output.tileCenterPointsENU.clear();
    // Convert and store scenario data.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
423
    const auto &tiles = this->pImpl->tilesENU;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
424 425 426 427 428
    for (unsigned int i = 0; i < tiles.size(); ++i) {
      const auto &tile = tiles[i];
      SnakeTile geoTile;
      SnakeTileLocal enuTile;
      for (size_t i = 0; i < tile.outer().size() - 1; ++i) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
429 430
        const auto &p = tile.outer()[i];
        QPointF enuVertex{p.get<0>(), p.get<1>()};
Valentin Platzgummer's avatar
Valentin Platzgummer committed
431
        QGeoCoordinate geoVertex;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
432
        snake::fromENU(this->pImpl->ENUOrigin, p, geoVertex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
433 434 435
        enuTile.polygon().points().push_back(enuVertex);
        geoTile.push_back(geoVertex);
      }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
436 437 438
      snake::BoostPoint centerPoint;
      snake::polygonCenter(tile, centerPoint);
      QPointF enuVertex(centerPoint.get<0>(), centerPoint.get<1>());
Valentin Platzgummer's avatar
Valentin Platzgummer committed
439
      QGeoCoordinate geoVertex;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
440
      snake::fromENU(this->pImpl->ENUOrigin, centerPoint, geoVertex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
441 442 443 444 445 446 447 448 449 450 451 452 453 454 455
      geoTile.setCenter(geoVertex);
      this->pImpl->output.tiles.polygons().push_back(geoTile);
      this->pImpl->output.tileCenterPoints.push_back(
          QVariant::fromValue(geoVertex));
      this->pImpl->output.tilesENU.polygons().push_back(enuTile);
      this->pImpl->output.tileCenterPointsENU.push_back(enuVertex);
    }
    this->pImpl->output.tilesUpdated = true;
  }
  // Store progress.
  this->pImpl->output.progressUpdated = false;
  if (!progressValid) {
    this->pImpl->input.progress.progress().clear();
    this->pImpl->output.progressUpdated = true;
  } else if (this->pImpl->input.progressChanged) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
456
    if (progress.size() == this->pImpl->tilesENU.size()) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482
      auto &qProgress = this->pImpl->input.progress;
      qProgress.progress().clear();
      for (const auto &p : progress) {
        qProgress.progress().push_back(p);
      }
    }
    this->pImpl->output.progressUpdated = true;
  }
  // Store waypoints.
  if (!waypointsValid) {
    // Clear waypoints.
    this->pImpl->output.arrivalPath.clear();
    this->pImpl->output.returnPath.clear();
    this->pImpl->output.arrivalPathENU.clear();
    this->pImpl->output.returnPathENU.clear();
    this->pImpl->output.waypoints.clear();
    this->pImpl->output.waypointsENU.clear();
    this->pImpl->output.waypointsUpdated = true;
  } else if (needWaypointUpdate) {
    // Clear waypoints.
    this->pImpl->output.arrivalPath.clear();
    this->pImpl->output.returnPath.clear();
    this->pImpl->output.arrivalPathENU.clear();
    this->pImpl->output.returnPathENU.clear();
    this->pImpl->output.waypoints.clear();
    this->pImpl->output.waypointsENU.clear();
483
    // Store arrival path.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
484 485 486 487
    const auto &info = transectsInfo.front();
    const auto &firstTransect = transects[info.index];
    const auto &firstWaypoint =
        info.reversed ? firstTransect.front() : firstTransect.back();
488 489 490 491 492 493 494 495 496
    long startIdx = 0;
    for (long i = 0; i < long(route.size()); ++i) {
      const auto &boostVertex = route[i];
      if (boostVertex == firstWaypoint) {
        startIdx = i;
        break;
      }
      QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
      QGeoCoordinate geoVertex;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
497
      snake::fromENU(this->pImpl->ENUOrigin, boostVertex, geoVertex);
498 499 500 501 502
      this->pImpl->output.arrivalPathENU.push_back(enuVertex);
      this->pImpl->output.arrivalPath.push_back(geoVertex);
    }
    // Store return path.
    long endIdx = 0;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
503 504 505 506
    const auto &info2 = transectsInfo.back();
    const auto &lastTransect = transects[info2.index];
    const auto &lastWaypoint =
        info2.reversed ? lastTransect.front() : lastTransect.back();
507 508 509 510 511
    for (long i = route.size() - 1; i >= 0; --i) {
      const auto &boostVertex = route[i];
      if (boostVertex == lastWaypoint) {
        endIdx = i;
        break;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
512
      }
513 514
      QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
      QGeoCoordinate geoVertex;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
515
      snake::fromENU(this->pImpl->ENUOrigin, boostVertex, geoVertex);
516 517
      this->pImpl->output.returnPathENU.push_back(enuVertex);
      this->pImpl->output.returnPath.push_back(geoVertex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
518
    }
519 520 521 522 523
    // Store waypoints.
    for (long i = startIdx; i <= endIdx; ++i) {
      const auto &boostVertex = route[i];
      QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
      QGeoCoordinate geoVertex;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
524
      snake::fromENU(this->pImpl->ENUOrigin, boostVertex, geoVertex);
525 526 527 528 529 530 531 532 533 534 535 536 537
      this->pImpl->output.waypointsENU.push_back(enuVertex);
      this->pImpl->output.waypoints.push_back(geoVertex);
    }
    // Store waypoints.
    //    for (const auto &t : transects) {
    //      for (const auto &v : t) {
    //        QPointF enuVertex{v.get<0>(), v.get<1>()};
    //        QGeoCoordinate geoVertex;
    //        snake::fromENU(origin, v, geoVertex);
    //        this->pImpl->output.waypointsENU.push_back(enuVertex);
    //        this->pImpl->output.waypoints.push_back(geoVertex);
    //      }
    //    }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
538 539 540
    this->pImpl->output.waypointsUpdated = true;
  }
}