CircularSurvey.cc 34 KB
Newer Older
1
#include "CircularSurvey.h"
2
#include "RoutingThread.h"
3
// QGC
4 5
#include "JsonHelper.h"
#include "QGCApplication.h"
6
// Wima
7
#include "snake.h"
8 9 10 11 12 13 14
#define CLIPPER_SCALE 10000
#include "clipper/clipper.hpp"

template <int k> ClipperLib::cInt get(ClipperLib::IntPoint &p);
template <> ClipperLib::cInt get<0>(ClipperLib::IntPoint &p) { return p.X; }
template <> ClipperLib::cInt get<1>(ClipperLib::IntPoint &p) { return p.Y; }
#include "Geometry/GenericCircle.h"
15
// boost
16 17 18 19 20 21 22 23 24 25 26 27
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>

template <class Functor> class CommandRAII {
public:
  CommandRAII(Functor f) : fun(f) {}
  ~CommandRAII() { fun(); }

private:
  Functor fun;
};

28 29 30 31 32
template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value) {
  return static_cast<typename std::underlying_type<T>::type>(value);
}

33
bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot,
34
                       bool useDepot, const QList<QGeoCoordinate> &polygon,
35 36 37
                       snake::Length deltaR, snake::Angle deltaAlpha,
                       snake::Length minLength, snake::Transects &transects);

38 39 40 41 42
bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot,
                     bool useDepot, const QList<QGeoCoordinate> &polygon,
                     snake::Length distance, snake::Angle angle,
                     snake::Length minLength, snake::Transects &transects);

43
const char *CircularSurvey::settingsGroup = "CircularSurvey";
44 45 46 47
const char *CircularSurvey::transectDistanceName = "TransectDistance";
const char *CircularSurvey::alphaName = "Alpha";
const char *CircularSurvey::minLengthName = "MinLength";
const char *CircularSurvey::typeName = "Type";
48 49 50 51 52 53 54 55 56 57 58
const char *CircularSurvey::CircularSurveyName = "CircularSurvey";
const char *CircularSurvey::refPointLatitudeName = "ReferencePointLat";
const char *CircularSurvey::refPointLongitudeName = "ReferencePointLong";
const char *CircularSurvey::refPointAltitudeName = "ReferencePointAlt";

CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
                               const QString &kmlOrShpFile, QObject *parent)
    : TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent),
      _referencePoint(QGeoCoordinate(0, 0, 0)),
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)),
59 60 61 62
      _transectDistance(settingsGroup, _metaDataMap[transectDistanceName]),
      _alpha(settingsGroup, _metaDataMap[alphaName]),
      _minLength(settingsGroup, _metaDataMap[minLengthName]),
      _type(settingsGroup, _metaDataMap[typeName]),
63
      _pWorker(std::make_unique<RoutingThread>()), _needsStoring(false),
64
      _needsReversal(false), _hidePolygon(false) {
65 66 67
  Q_UNUSED(kmlOrShpFile)
  _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";

68
  // Connect facts.
69
  connect(&_transectDistance, &Fact::valueChanged, this,
70
          &CircularSurvey::_rebuildTransects);
71
  connect(&_alpha, &Fact::valueChanged, this,
72
          &CircularSurvey::_rebuildTransects);
73
  connect(&_minLength, &Fact::valueChanged, this,
74
          &CircularSurvey::_rebuildTransects);
75
  connect(this, &CircularSurvey::refPointChanged, this,
76
          &CircularSurvey::_rebuildTransects);
77 78 79 80
  connect(this, &CircularSurvey::depotChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(this, &CircularSurvey::safeAreaChanged, this,
          &CircularSurvey::_rebuildTransects);
81 82
  connect(&this->_type, &Fact::rawValueChanged, this,
          &CircularSurvey::_rebuildTransects);
83
  // Connect worker.
84
  connect(this->_pWorker.get(), &RoutingThread::result, this,
85
          &CircularSurvey::_setTransects);
86
  connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this,
87 88
          &CircularSurvey::calculatingChanged);
  this->_transectsDirty = false;
89 90
}

91 92
CircularSurvey::~CircularSurvey() {}

93 94 95 96
void CircularSurvey::resetReference() {
  setRefPoint(_surveyAreaPolygon.center());
}

97 98 99 100 101
void CircularSurvey::reverse() {
  this->_needsReversal = true;
  this->_rebuildTransects();
}

102 103 104
void CircularSurvey::setRefPoint(const QGeoCoordinate &refPt) {
  if (refPt != _referencePoint) {
    _referencePoint = refPt;
105
    _referencePoint.setAltitude(0);
106 107 108 109 110 111 112

    emit refPointChanged();
  }
}

QGeoCoordinate CircularSurvey::refPoint() const { return _referencePoint; }

113
Fact *CircularSurvey::transectDistance() { return &_transectDistance; }
114

115
Fact *CircularSurvey::alpha() { return &_alpha; }
116

117 118 119 120 121 122 123 124
bool CircularSurvey::hidePolygon() const { return _hidePolygon; }

QGeoCoordinate CircularSurvey::depot() const { return this->_depot; }

QList<QGeoCoordinate> CircularSurvey::safeArea() const {
  return this->_safeArea;
}

125 126 127 128
const QList<QList<QGeoCoordinate>> &CircularSurvey::rawTransects() const {
  return this->_rawTransects;
}

129 130 131 132 133 134 135 136 137 138
void CircularSurvey::setHidePolygon(bool hide) {
  if (this->_hidePolygon != hide) {
    this->_hidePolygon = hide;
    emit hidePolygonChanged();
  }
}

void CircularSurvey::setDepot(const QGeoCoordinate &depot) {
  if (this->_depot != depot) {
    this->_depot = depot;
139
    this->_depot.setAltitude(0);
140 141 142 143 144 145 146 147 148 149
    emit depotChanged();
  }
}

void CircularSurvey::setSafeArea(const QList<QGeoCoordinate> &safeArea) {
  if (this->_safeArea != safeArea) {
    this->_safeArea = safeArea;
    emit safeAreaChanged();
  }
}
150 151 152

bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
                          QString &errorString) {
153 154
  // We need to pull version first to determine what validation/conversion
  // needs to be performed
155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171
  QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
      {JsonHelper::jsonVersionKey, QJsonValue::Double, true},
  };
  if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList,
                                errorString)) {
    return false;
  }

  int version = complexObject[JsonHelper::jsonVersionKey].toInt();
  if (version != 1) {
    errorString = tr("Survey items do not support version %1").arg(version);
    return false;
  }

  QList<JsonHelper::KeyValidateInfo> keyInfoList = {
      {VisualMissionItem::jsonTypeKey, QJsonValue::String, true},
      {ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true},
172 173 174 175
      {transectDistanceName, QJsonValue::Double, true},
      {alphaName, QJsonValue::Double, true},
      {minLengthName, QJsonValue::Double, true},
      {typeName, QJsonValue::Double, true},
176 177 178 179 180 181 182 183 184 185 186 187 188 189
      {refPointLatitudeName, QJsonValue::Double, true},
      {refPointLongitudeName, QJsonValue::Double, true},
      {refPointAltitudeName, QJsonValue::Double, true},
  };

  if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
    return false;
  }

  QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
  QString complexType =
      complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
  if (itemType != VisualMissionItem::jsonTypeComplexItemValue ||
      complexType != CircularSurveyName) {
190 191 192 193 194
    errorString = tr("%1 does not support loading this complex mission item "
                     "type: %2:%3")
                      .arg(qgcApp()->applicationName())
                      .arg(itemType)
                      .arg(complexType);
195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212
    return false;
  }

  _ignoreRecalc = true;

  setSequenceNumber(sequenceNumber);

  if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */,
                                       errorString)) {
    _surveyAreaPolygon.clear();
    return false;
  }

  if (!_load(complexObject, errorString)) {
    _ignoreRecalc = false;
    return false;
  }

213 214 215 216
  _transectDistance.setRawValue(complexObject[transectDistanceName].toDouble());
  _alpha.setRawValue(complexObject[alphaName].toDouble());
  _minLength.setRawValue(complexObject[minLengthName].toDouble());
  _type.setRawValue(complexObject[typeName].toInt());
217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245
  _referencePoint.setLongitude(complexObject[refPointLongitudeName].toDouble());
  _referencePoint.setLatitude(complexObject[refPointLatitudeName].toDouble());
  _referencePoint.setAltitude(complexObject[refPointAltitudeName].toDouble());

  _ignoreRecalc = false;

  _recalcComplexDistance();
  if (_cameraShots == 0) {
    // Shot count was possibly not available from plan file
    _recalcCameraShots();
  }

  return true;
}

QString CircularSurvey::mapVisualQML() const {
  return QStringLiteral("CircularSurveyMapVisual.qml");
}

void CircularSurvey::save(QJsonArray &planItems) {
  QJsonObject saveObject;

  _save(saveObject);

  saveObject[JsonHelper::jsonVersionKey] = 1;
  saveObject[VisualMissionItem::jsonTypeKey] =
      VisualMissionItem::jsonTypeComplexItemValue;
  saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = CircularSurveyName;

246 247 248 249
  saveObject[transectDistanceName] = _transectDistance.rawValue().toDouble();
  saveObject[alphaName] = _alpha.rawValue().toDouble();
  saveObject[minLengthName] = _minLength.rawValue().toDouble();
  saveObject[typeName] = double(_type.rawValue().toUInt());
250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289
  saveObject[refPointLongitudeName] = _referencePoint.longitude();
  saveObject[refPointLatitudeName] = _referencePoint.latitude();
  saveObject[refPointAltitudeName] = _referencePoint.altitude();

  // Polygon shape
  _surveyAreaPolygon.saveToJson(saveObject);

  planItems.append(saveObject);
}

bool CircularSurvey::specifiesCoordinate() const { return true; }

void CircularSurvey::appendMissionItems(QList<MissionItem *> &items,
                                        QObject *missionItemParent) {
  if (_transectsDirty)
    return;
  if (_loadedMissionItems.count()) {
    // We have mission items from the loaded plan, use those
    _appendLoadedMissionItems(items, missionItemParent);
  } else {
    // Build the mission items on the fly
    _buildAndAppendMissionItems(items, missionItemParent);
  }
}

void CircularSurvey::_appendLoadedMissionItems(QList<MissionItem *> &items,
                                               QObject *missionItemParent) {
  if (_transectsDirty)
    return;
  int seqNum = _sequenceNumber;

  for (const MissionItem *loadedMissionItem : _loadedMissionItems) {
    MissionItem *item = new MissionItem(*loadedMissionItem, missionItemParent);
    item->setSequenceNumber(seqNum++);
    items.append(item);
  }
}

void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
                                                 QObject *missionItemParent) {
290
  if (_transectsDirty || _transects.count() == 0)
291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307
    return;

  MissionItem *item;
  int seqNum = _sequenceNumber;

  MAV_FRAME mavFrame =
      followTerrain() || !_cameraCalc.distanceToSurfaceRelative()
          ? MAV_FRAME_GLOBAL
          : MAV_FRAME_GLOBAL_RELATIVE_ALT;

  for (const QList<TransectStyleComplexItem::CoordInfo_t> &transect :
       _transects) {
    // bool transectEntry = true;

    for (const CoordInfo_t &transectCoordInfo : transect) {
      item = new MissionItem(
          seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame,
308 309
          0,   // Hold time (delay for hover and capture to settle vehicle
               // before image is taken)
310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346
          0.0, // No acceptance radius specified
          0.0, // Pass through waypoint
          std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
          transectCoordInfo.coord.latitude(),
          transectCoordInfo.coord.longitude(),
          transectCoordInfo.coord.altitude(),
          true,  // autoContinue
          false, // isCurrentItem
          missionItemParent);
      items.append(item);
    }
  }
}

void CircularSurvey::applyNewAltitude(double newAltitude) {
  _cameraCalc.valueSetIsDistance()->setRawValue(true);
  _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
  _cameraCalc.setDistanceToSurfaceRelative(true);
}

double CircularSurvey::timeBetweenShots() { return 1; }

QString CircularSurvey::commandDescription() const {
  return tr("Circular Survey");
}

QString CircularSurvey::commandName() const { return tr("Circular Survey"); }

QString CircularSurvey::abbreviation() const { return tr("C.S."); }

bool CircularSurvey::readyForSave() const {
  return TransectStyleComplexItem::readyForSave() && !_transectsDirty;
}

double CircularSurvey::additionalTimeDelay() const { return 0; }

void CircularSurvey::_rebuildTransectsPhase1(void) {
347
  // Store result of former calculation.
348
  if (this->_needsStoring) {
349 350 351
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    auto start = std::chrono::high_resolution_clock::now();
#endif
352 353 354 355 356 357
    // If the transects are getting rebuilt then any previously loaded
    // mission items are now invalid.
    if (_loadedMissionItemsParent) {
      _loadedMissionItems.clear();
      _loadedMissionItemsParent->deleteLater();
      _loadedMissionItemsParent = nullptr;
358
    }
359 360 361
    // Store raw transects.
    const auto &transectsENU = this->_workerOutput->transects;
    const auto &ori = this->_referencePoint;
362 363 364 365 366 367
    std::size_t startIdx = 0;
    if (transectsENU.size() > 0 && transectsENU.front().size() == 1) {
      startIdx = 1;
    }
    for (std::size_t i = startIdx; i < transectsENU.size(); ++i) {
      const auto &t = transectsENU[i];
368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406
      QList<QGeoCoordinate> trGeo;
      for (auto &v : t) {
        QGeoCoordinate c;
        snake::fromENU(ori, v, c);
        trGeo.append(c);
      }
      this->_rawTransects.append(trGeo);
    }
    // Store route.
    const auto &transectsInfo = this->_workerOutput->transectsInfo;
    const auto &route = this->_workerOutput->route;
    // Find index of first waypoint.
    std::size_t idxFirst = 0;
    const auto &infoFirst = transectsInfo.front();
    const auto &firstTransect = transectsENU[infoFirst.index];
    const auto &firstWaypoint =
        infoFirst.reversed ? firstTransect.back() : firstTransect.front();
    double th = 0.001;
    for (std::size_t i = 0; i < route.size(); ++i) {
      auto dist = bg::distance(route[i], firstWaypoint);
      if (dist < th) {
        idxFirst = i;
        break;
      }
    }
    // Find index of last waypoint.
    std::size_t idxLast = route.size() - 1;
    const auto &infoLast = transectsInfo.back();
    const auto &lastTransect = transectsENU[infoLast.index];
    const auto &lastWaypoint =
        infoLast.reversed ? lastTransect.front() : lastTransect.back();
    for (long i = route.size() - 1; i >= 0; --i) {
      auto dist = bg::distance(route[i], lastWaypoint);
      if (dist < th) {
        idxLast = i;
        break;
      }
    }
    // Convert to geo coordinates.
407
    QList<CoordInfo_t> list;
408
    for (std::size_t i = idxFirst; i <= idxLast; ++i) {
409
      auto &vertex = route[i];
410 411
      QGeoCoordinate c;
      snake::fromENU(ori, vertex, c);
412
      list.append(CoordInfo_t{c, CoordTypeInterior});
413
    }
414
    this->_transects.append(list);
415
    // Mark transect as stored and ready.
416
    this->_needsStoring = false;
417 418 419 420 421 422 423 424 425 426 427
    this->_transectsDirty = false;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    qWarning() << "CS::rebuildTransectsPhase1(): store: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(
                      std::chrono::high_resolution_clock::now() - start)
                      .count()
               << " ms";
#endif
  }
  // Reverse transects only.
  else if (this->_needsReversal) {
428 429 430 431 432 433 434 435 436
    if (this->_transects.size() > 0) {
      auto &t = this->_transects.front();
      QList<CoordInfo_t> list;
      list.reserve(t.size());
      for (auto it = t.end() - 1; it >= t.begin(); --it) {
        list.append(*it);
      }
      this->_transects.clear();
      this->_transects.append(list);
437
    }
438
    this->_needsReversal = false;
439 440 441 442 443 444
  }
  // Start calculation.
  else {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    auto start = std::chrono::high_resolution_clock::now();
#endif
445
    this->_transects.clear();
446
    this->_rawTransects.clear();
447 448
    // Prepare data.
    auto ref = this->_referencePoint;
449
    auto polygon = this->_surveyAreaPolygon.coordinateList();
450 451 452 453 454 455 456
    for (auto &v : polygon) {
      v.setAltitude(0);
    }
    auto safeArea = this->_safeArea;
    for (auto &v : safeArea) {
      v.setAltitude(0);
    }
457
    auto depot = this->_depot;
458
    snake::BoostPolygon safeAreaENU;
459
    bool useDepot = false;
460
    if (this->_depot.isValid() && this->_safeArea.size() >= 3) {
461
      useDepot = true;
462
      snake::areaToEnu(ref, safeArea, safeAreaENU);
463
    } else {
464
      snake::areaToEnu(ref, polygon, safeAreaENU);
465
    }
466 467
    auto distance = snake::Length(
        this->_transectDistance.rawValue().toDouble() * bu::si::meter);
468 469
    auto minLength =
        snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493
    auto alpha =
        snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);
    // Select survey type.
    if (this->_type.rawValue().toUInt() == integral(Type::Circular)) {
      if (alpha >= snake::Angle(0.3 * bu::degree::degree) &&
          alpha <= snake::Angle(45 * bu::degree::degree)) {
        auto generator = [ref, depot, useDepot, polygon, distance, alpha,
                          minLength](snake::Transects &transects) -> bool {
          return circularTransects(ref, depot, useDepot, polygon, distance,
                                   alpha, minLength, transects);
        };
        // Start routing worker.
        this->_pWorker->route(safeAreaENU, generator);
      } else {
        if (alpha < snake::Angle(0.3 * bu::degree::degree)) {
          this->_alpha.setCookedValue(QVariant(0.3));
        } else {
          this->_alpha.setCookedValue(QVariant(45));
        }
      }
    } else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) {
      auto generator = [ref, depot, useDepot, polygon, distance, alpha,
                        minLength](snake::Transects &transects) -> bool {
        return linearTransects(ref, depot, useDepot, polygon, distance, alpha,
494
                               minLength, transects);
495 496 497 498 499 500 501 502
      };
      // Start routing worker.
      this->_pWorker->route(safeAreaENU, generator);
    } else {
      qWarning()
          << "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:"
          << this->_type.rawValue().toUInt();
    }
503 504 505 506 507 508 509 510 511
    // Mark transects as dirty.
    this->_transectsDirty = true;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    qWarning() << "CS::rebuildTransectsPhase1(): start: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(
                      std::chrono::high_resolution_clock::now() - start)
                      .count()
               << " ms";
#endif
512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529
  }
}

void CircularSurvey::_recalcComplexDistance() {
  _complexDistance = 0;
  if (_transectsDirty)
    return;
  for (int i = 0; i < _visualTransectPoints.count() - 1; i++) {
    _complexDistance +=
        _visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(
            _visualTransectPoints[i + 1].value<QGeoCoordinate>());
  }
  emit complexDistanceChanged();
}

// no cameraShots in Circular Survey, add if desired
void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }

530 531
void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
  this->_workerOutput = pRoute;
532 533
  this->_needsStoring = true;
  this->_rebuildTransects();
534 535
}

536 537 538 539 540
Fact *CircularSurvey::minLength() { return &_minLength; }

Fact *CircularSurvey::type() { return &_type; }

int CircularSurvey::typeCount() const { return int(integral(Type::Count)); }
541

542 543 544
bool CircularSurvey::calculating() const {
  return this->_pWorker->calculating();
}
545

546
bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot,
547
                       bool useDepot, const QList<QGeoCoordinate> &polygon,
548 549 550 551 552 553 554 555 556 557 558 559 560 561
                       snake::Length deltaR, snake::Angle deltaAlpha,
                       snake::Length minLength, snake::Transects &transects) {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
  auto s1 = std::chrono::high_resolution_clock::now();
#endif
  // Check preconitions
  if (polygon.size() >= 3) {
    using namespace boost::units;
    // Convert geo polygon to ENU polygon.
    snake::BoostPolygon polygonENU;
    snake::BoostPoint originENU{0, 0};
    snake::BoostPoint depotENU{0, 0};
    snake::areaToEnu(ref, polygon, polygonENU);
    snake::toENU(ref, ref, originENU);
562
    snake::toENU(ref, depot, depotENU);
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 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygonENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
      qWarning() << "CS::circularTransects(): "
                    "invalid polygon.";
      qWarning() << error.c_str();
      std::stringstream ss;
      ss << bg::wkt(polygonENU);
      qWarning() << ss.str().c_str();
#endif
    } else {
      // Calculate polygon distances and angles.
      std::vector<snake::Length> distances;
      distances.reserve(polygonENU.outer().size());
      std::vector<snake::Angle> angles;
      angles.reserve(polygonENU.outer().size());
      //#ifdef DEBUG_CIRCULAR_SURVEY
      //      qWarning() << "CS::circularTransects():";
      //#endif
      for (const auto &p : polygonENU.outer()) {
        snake::Length distance = bg::distance(originENU, p) * si::meter;
        distances.push_back(distance);
        snake::Angle alpha = (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
        alpha = alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
        angles.push_back(alpha);
        //#ifdef DEBUG_CIRCULAR_SURVEY
        //        qWarning() << "distances, angles, coordinates:";
        //        qWarning() << to_string(distance).c_str();
        //        qWarning() << to_string(snake::Degree(alpha)).c_str();
        //        qWarning() << "x = " << p.get<0>() << "y = " << p.get<1>();
        //#endif
      }

      auto rMin = deltaR; // minimal circle radius
      snake::Angle alpha1(0 * degree::degree);
      snake::Angle alpha2(360 * degree::degree);
      // Determine r_min by successive approximation
      if (!bg::within(originENU, polygonENU)) {
        rMin = bg::distance(originENU, polygonENU) * si::meter;
      }

      auto rMax = (*std::max_element(distances.begin(),
                                     distances.end())); // maximal circle radius

      // Scale parameters and coordinates.
      const auto rMinScaled =
          ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
      const auto deltaRScaled =
          ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
      auto originScaled = ClipperLib::IntPoint{
          ClipperLib::cInt(std::round(originENU.get<0>())),
          ClipperLib::cInt(std::round(originENU.get<1>()))};

      // Generate circle sectors.
      auto rScaled = rMinScaled;
      const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
      vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
      const auto nSectors =
          long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
      //#ifdef DEBUG_CIRCULAR_SURVEY
      //      qWarning() << "CS::circularTransects(): sector parameres:";
      //      qWarning() << "alpha1: " <<
      //      to_string(snake::Degree(alpha1)).c_str(); qWarning() << "alpha2: "
      //      << to_string(snake::Degree(alpha2)).c_str(); qWarning() << "n: "
      //      << to_string((alpha2 - alpha1) / deltaAlpha).c_str(); qWarning()
      //      << "nSectors: " << nSectors; qWarning() << "rMin: " <<
      //      to_string(rMin).c_str(); qWarning() << "rMax: " <<
      //      to_string(rMax).c_str(); qWarning() << "nTran: " << nTran;
      //#endif
      using ClipperCircle =
          GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
      for (auto &sector : sectors) {
        ClipperCircle circle(rScaled, originScaled);
        approximate(circle, nSectors, sector);
        rScaled += deltaRScaled;
      }
      // Clip sectors to polygonENU.
      ClipperLib::Path polygonClipper;
      snake::BoostPolygon shrinked;
      snake::offsetPolygon(polygonENU, shrinked, -0.3);
      auto &outer = shrinked.outer();
645
      polygonClipper.reserve(outer.size());
646 647 648 649 650 651 652 653 654 655 656 657 658 659
      for (auto it = outer.begin(); it < outer.end() - 1; ++it) {
        auto x = ClipperLib::cInt(std::round(it->get<0>() * CLIPPER_SCALE));
        auto y = ClipperLib::cInt(std::round(it->get<1>() * CLIPPER_SCALE));
        polygonClipper.push_back(ClipperLib::IntPoint{x, y});
      }
      ClipperLib::Clipper clipper;
      clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
      clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
      ClipperLib::PolyTree transectsClipper;
      clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
                      ClipperLib::pftNonZero, ClipperLib::pftNonZero);

      // Extract transects from  PolyTree and convert them to
      // BoostLineString
660
      if (useDepot) {
661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719
        transects.push_back(snake::BoostLineString{depotENU});
      }
      for (const auto &child : transectsClipper.Childs) {
        snake::BoostLineString transect;
        transect.reserve(child->Contour.size());
        for (const auto &vertex : child->Contour) {
          auto x = static_cast<double>(vertex.X) / CLIPPER_SCALE;
          auto y = static_cast<double>(vertex.Y) / CLIPPER_SCALE;
          transect.push_back(snake::BoostPoint(x, y));
        }
        transects.push_back(transect);
      }
      // Join sectors which where slit due to clipping.
      const double th = 0.01;
      for (auto ito = transects.begin(); ito < transects.end(); ++ito) {
        for (auto iti = ito + 1; iti < transects.end(); ++iti) {
          auto dist1 = bg::distance(ito->front(), iti->front());
          if (dist1 < th) {
            snake::BoostLineString temp;
            for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
              temp.push_back(*it);
            }
            temp.insert(temp.end(), ito->begin(), ito->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist2 = bg::distance(ito->front(), iti->back());
          if (dist2 < th) {
            snake::BoostLineString temp;
            temp.insert(temp.end(), iti->begin(), iti->end());
            temp.insert(temp.end(), ito->begin(), ito->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist3 = bg::distance(ito->back(), iti->front());
          if (dist3 < th) {
            snake::BoostLineString temp;
            temp.insert(temp.end(), ito->begin(), ito->end());
            temp.insert(temp.end(), iti->begin(), iti->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist4 = bg::distance(ito->back(), iti->back());
          if (dist4 < th) {
            snake::BoostLineString temp;
            temp.insert(temp.end(), ito->begin(), ito->end());
            for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
              temp.push_back(*it);
            }
            *ito = temp;
            transects.erase(iti);
            break;
          }
        }
      }
      // Remove short transects
720
      auto begin = useDepot ? transects.begin() + 1 : transects.begin();
721 722 723 724 725 726 727 728
      for (auto it = begin; it < transects.end();) {
        if (bg::length(*it) < minLength.value()) {
          it = transects.erase(it);
        } else {
          ++it;
        }
      }

729
      if (!useDepot) {
730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774
        // Move transect with min. distance to the front.
        auto minDist = std::numeric_limits<double>::max();
        auto minIt = transects.begin();
        bool reverse = false;
        for (auto it = transects.begin(); it < transects.end(); ++it) {
          auto distFront = bg::distance(originENU, it->front());
          auto distBack = bg::distance(originENU, it->back());
          if (distFront < minDist) {
            minDist = distFront;
            minIt = it;
            reverse = false;
          }
          if (distBack < minDist) {
            minDist = distBack;
            minIt = it;
            reverse = true;
          }
        }
        // Swap and reverse (if necessary).
        if (minIt != transects.begin()) {
          auto minTransect = *minIt;
          if (reverse) {
            snake::BoostLineString rev;
            for (auto it = minTransect.end() - 1; it >= minTransect.begin();
                 --it) {
              rev.push_back(*it);
            }
            minTransect = rev;
          }
          *minIt = *transects.begin();
          *transects.begin() = minTransect;
        }
      }
#ifdef SHOW_CIRCULAR_SURVEY_TIME
      qWarning() << "CS::circularTransects(): transect gen. time: "
                 << std::chrono::duration_cast<std::chrono::milliseconds>(
                        std::chrono::high_resolution_clock::now() - s1)
                        .count()
                 << " ms";
#endif
      return true;
    }
  }
  return false;
}
775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909

bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot,
                     bool useDepot, const QList<QGeoCoordinate> &polygon,
                     snake::Length distance, snake::Angle angle,
                     snake::Length minLength, snake::Transects &transects) {
  namespace tr = bg::strategy::transform;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
  auto s1 = std::chrono::high_resolution_clock::now();
#endif
  // Check preconitions
  if (polygon.size() >= 3) {
    // Convert to ENU system.
    snake::BoostPolygon polygonENU;
    snake::areaToEnu(origin, polygon, polygonENU);
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygonENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
      qWarning() << "CS::circularTransects(): "
                    "invalid polygon.";
      qWarning() << error.c_str();
      std::stringstream ss;
      ss << bg::wkt(polygonENU);
      qWarning() << ss.str().c_str();
#endif
    } else {
      snake::BoostPoint depotENU;
      snake::toENU(origin, depot, depotENU);
      tr::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
                                                              180 / M_PI);
      // Rotate polygon by angle and calculate bounding box.
      snake::BoostPolygon polygonENURotated;
      bg::transform(polygonENU, polygonENURotated, rotate);
      snake::BoostBox box;
      boost::geometry::envelope(polygonENURotated, box);
      double x0 = box.min_corner().get<0>();
      double y0 = box.min_corner().get<1>();
      double x1 = box.max_corner().get<0>();
      double y1 = box.max_corner().get<1>();

      // Generate transects and convert them to clipper path.
      size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects
      vector<ClipperLib::Path> transectsClipper;
      transectsClipper.reserve(num_t);
      for (size_t i = 0; i < num_t; ++i) {
        // calculate transect
        snake::BoostPoint v1{x0, y0 + i * distance.value()};
        snake::BoostPoint v2{x1, y0 + i * distance.value()};
        snake::BoostLineString transect;
        transect.push_back(v1);
        transect.push_back(v2);
        // transform back
        snake::BoostLineString temp_transect;
        tr::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
            -angle.value() * 180 / M_PI);
        bg::transform(transect, temp_transect, rotate_back);
        // to clipper
        ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
                                    temp_transect[0].get<0>() * CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(
                                    temp_transect[0].get<1>() * CLIPPER_SCALE)};
        ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
                                    temp_transect[1].get<0>() * CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(
                                    temp_transect[1].get<1>() * CLIPPER_SCALE)};
        ClipperLib::Path path{c1, c2};
        transectsClipper.push_back(path);
      }

      if (transectsClipper.size() == 0) {
        std::stringstream ss;
        ss << "Not able to generate transects. Parameter: distance = "
           << distance << std::endl;
        qWarning() << "CircularSurvey::linearTransects(): " << ss.str().c_str();
        return false;
      }

      // Convert measurement area to clipper path.
      snake::BoostPolygon shrinked;
      snake::offsetPolygon(polygonENU, shrinked, -0.2);
      auto &outer = shrinked.outer();
      ClipperLib::Path polygonClipper;
      for (auto vertex : outer) {
        polygonClipper.push_back(ClipperLib::IntPoint{
            static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
            static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
      }

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

      // Extract transects from  PolyTree and convert them to BoostLineString
      if (useDepot) {
        transects.push_back(snake::BoostLineString{depotENU});
      }
      for (const auto &child : clippedTransecs.Childs) {
        const auto &clipperTransect = child->Contour;
        snake::BoostPoint v1{
            static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
            static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
        snake::BoostPoint v2{
            static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
            static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};

        snake::BoostLineString transect{v1, v2};
        if (bg::length(transect) >= minLength.value()) {
          transects.push_back(transect);
        }
      }

      if (transects.size() == 0) {
        std::stringstream ss;
        ss << "Not able to generate transects. Parameter: minLength = "
           << minLength << std::endl;
        qWarning() << "CircularSurvey::linearTransects(): " << ss.str().c_str();
        return false;
      }
#ifdef SHOW_CIRCULAR_SURVEY_TIME
      qWarning() << "CS::circularTransects(): transect gen. time: "
                 << std::chrono::duration_cast<std::chrono::milliseconds>(
                        std::chrono::high_resolution_clock::now() - s1)
                        .count()
                 << " ms";
#endif
      return true;
    }
  }
  return false;
}
910 911 912 913
/*!
    \class CircularSurveyComplexItem
    \inmodule Wima

914 915
    \brief The \c CircularSurveyComplexItem class provides a survey mission
   item with circular transects around a point of interest.
916

917 918 919 920
    CircularSurveyComplexItem class provides a survey mission item with
   circular transects around a point of interest. Within the \c Wima module
   it's used to scan a defined area with constant angle (circular transects)
   to the base station (point of interest).
921 922 923

    \sa WimaArea
*/