SurveyComplexItem.cc 79.6 KB
Newer Older
1 2 3 4 5 6 7 8 9 10
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/


11
#include "SurveyComplexItem.h"
12 13 14
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
15
#include "QGroundControlQmlGlobal.h"
16
#include "QGCQGeoCoordinate.h"
17 18
#include "SettingsManager.h"
#include "AppSettings.h"
19 20 21

#include <QPolygonF>

22
QGC_LOGGING_CATEGORY(SurveyComplexItemLog, "SurveyComplexItemLog")
23
QGC_LOGGING_CATEGORY(PolygonDecomposeLog, "PolygonDecomposeLog")
24

25 26 27 28 29 30
const char* SurveyComplexItem::jsonComplexItemTypeValue =   "survey";
const char* SurveyComplexItem::jsonV3ComplexItemTypeValue = "survey";

const char* SurveyComplexItem::settingsGroup =              "Survey";
const char* SurveyComplexItem::gridAngleName =              "GridAngle";
const char* SurveyComplexItem::gridEntryLocationName =      "GridEntryLocation";
31
const char* SurveyComplexItem::flyAlternateTransectsName =  "FlyAlternateTransects";
32 33

const char* SurveyComplexItem::_jsonGridAngleKey =          "angle";
DonLakeFlyer's avatar
DonLakeFlyer committed
34
const char* SurveyComplexItem::_jsonEntryPointKey =         "entryLocation";
35 36 37 38 39 40

const char* SurveyComplexItem::_jsonV3GridObjectKey =                   "grid";
const char* SurveyComplexItem::_jsonV3GridAltitudeKey =                 "altitude";
const char* SurveyComplexItem::_jsonV3GridAltitudeRelativeKey =         "relativeAltitude";
const char* SurveyComplexItem::_jsonV3GridAngleKey =                    "angle";
const char* SurveyComplexItem::_jsonV3GridSpacingKey =                  "spacing";
DonLakeFlyer's avatar
DonLakeFlyer committed
41
const char* SurveyComplexItem::_jsonV3EntryPointKey =                   "entryLocation";
42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60
const char* SurveyComplexItem::_jsonV3TurnaroundDistKey =               "turnAroundDistance";
const char* SurveyComplexItem::_jsonV3CameraTriggerDistanceKey =        "cameraTriggerDistance";
const char* SurveyComplexItem::_jsonV3CameraTriggerInTurnaroundKey =    "cameraTriggerInTurnaround";
const char* SurveyComplexItem::_jsonV3HoverAndCaptureKey =              "hoverAndCapture";
const char* SurveyComplexItem::_jsonV3GroundResolutionKey =             "groundResolution";
const char* SurveyComplexItem::_jsonV3FrontalOverlapKey =               "imageFrontalOverlap";
const char* SurveyComplexItem::_jsonV3SideOverlapKey =                  "imageSideOverlap";
const char* SurveyComplexItem::_jsonV3CameraSensorWidthKey =            "sensorWidth";
const char* SurveyComplexItem::_jsonV3CameraSensorHeightKey =           "sensorHeight";
const char* SurveyComplexItem::_jsonV3CameraResolutionWidthKey =        "resolutionWidth";
const char* SurveyComplexItem::_jsonV3CameraResolutionHeightKey =       "resolutionHeight";
const char* SurveyComplexItem::_jsonV3CameraFocalLengthKey =            "focalLength";
const char* SurveyComplexItem::_jsonV3CameraMinTriggerIntervalKey =     "minTriggerInterval";
const char* SurveyComplexItem::_jsonV3CameraObjectKey =                 "camera";
const char* SurveyComplexItem::_jsonV3CameraNameKey =                   "name";
const char* SurveyComplexItem::_jsonV3ManualGridKey =                   "manualGrid";
const char* SurveyComplexItem::_jsonV3CameraOrientationLandscapeKey =   "orientationLandscape";
const char* SurveyComplexItem::_jsonV3FixedValueIsAltitudeKey =         "fixedValueIsAltitude";
const char* SurveyComplexItem::_jsonV3Refly90DegreesKey =               "refly90Degrees";
61
const char* SurveyComplexItem::_jsonFlyAlternateTransectsKey =          "flyAlternateTransects";
62

63
SurveyComplexItem::SurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent)
64
    : TransectStyleComplexItem  (vehicle, flyView, settingsGroup, parent)
65 66
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/Survey.SettingsGroup.json"), this))
    , _gridAngleFact            (settingsGroup, _metaDataMap[gridAngleName])
67
    , _flyAlternateTransectsFact(settingsGroup, _metaDataMap[flyAlternateTransectsName])
DonLakeFlyer's avatar
DonLakeFlyer committed
68
    , _entryPoint               (EntryLocationTopLeft)
69
{
70 71
    _editorQml = "qrc:/qml/SurveyItemEditor.qml";

72
    // If the user hasn't changed turnaround from the default (which is a fixed wing default) and we are multi-rotor set the multi-rotor default.
73
    // NULL check since object creation during unit testing passes NULL for vehicle
74
    if (_vehicle && _vehicle->multiRotor() && _turnAroundDistanceFact.rawValue().toDouble() == _turnAroundDistanceFact.rawDefaultValue().toDouble()) {
75
        // Note this is set to 10 meters to work around a problem with PX4 Pro turnaround behavior. Don't change unless firmware gets better as well.
76
        _turnAroundDistanceFact.setRawValue(10);
77
    }
Don Gagne's avatar
Don Gagne committed
78

79 80 81
    // We override the altitude to the mission default
    if (_cameraCalc.isManualCamera() || !_cameraCalc.valueSetIsDistance()->rawValue().toBool()) {
        _cameraCalc.distanceToSurface()->setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue());
82 83
    }

84
    connect(&_gridAngleFact,            &Fact::valueChanged,                        this, &SurveyComplexItem::_setDirty);
85
    connect(&_flyAlternateTransectsFact,&Fact::valueChanged,                        this, &SurveyComplexItem::_setDirty);
86
    connect(this,                       &SurveyComplexItem::refly90DegreesChanged,  this, &SurveyComplexItem::_setDirty);
87

88
    connect(&_gridAngleFact,            &Fact::valueChanged,                        this, &SurveyComplexItem::_rebuildTransects);
89
    connect(&_flyAlternateTransectsFact,&Fact::valueChanged,                        this, &SurveyComplexItem::_rebuildTransects);
90
    connect(this,                       &SurveyComplexItem::refly90DegreesChanged,  this, &SurveyComplexItem::_rebuildTransects);
91

92 93
    connect(qgcApp()->toolbox()->settingsManager()->appSettings()->splitConcavePolygons(), &Fact::valueChanged, this, &SurveyComplexItem::_rebuildTransects);

94 95 96
    // FIXME: Shouldn't these be in TransectStyleComplexItem? They are also in CorridorScanComplexItem constructur
    connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &SurveyComplexItem::coordinateHasRelativeAltitudeChanged);
    connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &SurveyComplexItem::exitCoordinateHasRelativeAltitudeChanged);
97 98 99 100 101 102

    if (!kmlFile.isEmpty()) {
        _surveyAreaPolygon.loadKMLFile(kmlFile);
        _surveyAreaPolygon.setDirty(false);
    }
    setDirty(false);
103 104
}

105
void SurveyComplexItem::save(QJsonArray&  planItems)
106
{
107
    QJsonObject saveObject;
108

109
    _save(saveObject);
110

111 112 113 114
    saveObject[JsonHelper::jsonVersionKey] =                    4;
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
    saveObject[_jsonGridAngleKey] =                             _gridAngleFact.rawValue().toDouble();
115
    saveObject[_jsonFlyAlternateTransectsKey] =                 _flyAlternateTransectsFact.rawValue().toBool();
DonLakeFlyer's avatar
DonLakeFlyer committed
116
    saveObject[_jsonEntryPointKey] =                            _entryPoint;
117

118 119
    // Polygon shape
    _surveyAreaPolygon.saveToJson(saveObject);
120

121
    planItems.append(saveObject);
122 123
}

124
bool SurveyComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
125
{
126 127 128 129 130 131
    // We need to pull version first to determine what validation/conversion needs to be performed
    QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
        { JsonHelper::jsonVersionKey, QJsonValue::Double, true },
    };
    if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList, errorString)) {
        return false;
132 133
    }

134 135 136 137
    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
    if (version < 2 || version > 4) {
        errorString = tr("Survey items do not support version %1").arg(version);
        return false;
138
    }
139

140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156
    if (version == 4) {
        if (!_loadV4(complexObject, sequenceNumber, errorString)) {
            return false;
        }
    } else {
        // Must be v2 or v3
        QJsonObject v3ComplexObject = complexObject;
        if (version == 2) {
            // Convert to v3
            if (v3ComplexObject.contains(VisualMissionItem::jsonTypeKey) && v3ComplexObject[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("survey")) {
                v3ComplexObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
                v3ComplexObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;
            }
        }
        if (!_loadV3(complexObject, sequenceNumber, errorString)) {
            return false;
        }
Don Gagne's avatar
Don Gagne committed
157
    }
158

159
    _rebuildTransects();
160

161
    return true;
162 163
}

164
bool SurveyComplexItem::_loadV4(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
165
{
166 167 168
    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
169
        { _jsonEntryPointKey,                           QJsonValue::Double, true },
170
        { _jsonGridAngleKey,                            QJsonValue::Double, true },
171
        { _jsonFlyAlternateTransectsKey,                QJsonValue::Bool,   false },
172 173 174
    };
    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
        return false;
175
    }
Don Gagne's avatar
Don Gagne committed
176

177 178 179 180
    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
181 182
        return false;
    }
Don Gagne's avatar
Don Gagne committed
183

184 185 186 187 188 189
    _ignoreRecalc = true;

    setSequenceNumber(sequenceNumber);

    if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) {
        _surveyAreaPolygon.clear();
190 191
        return false;
    }
192 193 194 195

    if (!_load(complexObject, errorString)) {
        _ignoreRecalc = false;
        return false;
Don Gagne's avatar
Don Gagne committed
196
    }
197

198 199 200
    _gridAngleFact.setRawValue              (complexObject[_jsonGridAngleKey].toDouble());
    _flyAlternateTransectsFact.setRawValue  (complexObject[_jsonFlyAlternateTransectsKey].toBool(false));

DonLakeFlyer's avatar
DonLakeFlyer committed
201
    _entryPoint = complexObject[_jsonEntryPointKey].toInt();
202 203 204 205 206 207 208 209

    _ignoreRecalc = false;

    return true;
}

bool SurveyComplexItem::_loadV3(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
Don Gagne's avatar
Don Gagne committed
210 211 212
    QList<JsonHelper::KeyValidateInfo> mainKeyInfoList = {
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
213
        { QGCMapPolygon::jsonPolygonKey,                QJsonValue::Array,  true },
214 215 216 217 218 219 220 221
        { _jsonV3GridObjectKey,                         QJsonValue::Object, true },
        { _jsonV3CameraObjectKey,                       QJsonValue::Object, false },
        { _jsonV3CameraTriggerDistanceKey,              QJsonValue::Double, true },
        { _jsonV3ManualGridKey,                         QJsonValue::Bool,   true },
        { _jsonV3FixedValueIsAltitudeKey,               QJsonValue::Bool,   true },
        { _jsonV3HoverAndCaptureKey,                    QJsonValue::Bool,   false },
        { _jsonV3Refly90DegreesKey,                     QJsonValue::Bool,   false },
        { _jsonV3CameraTriggerInTurnaroundKey,          QJsonValue::Bool,   false },    // Should really be required, but it was missing from initial code due to bug
Don Gagne's avatar
Don Gagne committed
222
    };
223
    if (!JsonHelper::validateKeys(complexObject, mainKeyInfoList, errorString)) {
224 225
        return false;
    }
Don Gagne's avatar
Don Gagne committed
226

227 228 229
    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonV3ComplexItemTypeValue) {
230
        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
231 232 233
        return false;
    }

234 235
    _ignoreRecalc = true;

Don Gagne's avatar
Don Gagne committed
236
    setSequenceNumber(sequenceNumber);
237

238 239 240 241 242 243
    _hoverAndCaptureFact.setRawValue            (complexObject[_jsonV3HoverAndCaptureKey].toBool(false));
    _refly90DegreesFact.setRawValue             (complexObject[_jsonV3Refly90DegreesKey].toBool(false));
    _cameraTriggerInTurnAroundFact.setRawValue  (complexObject[_jsonV3CameraTriggerInTurnaroundKey].toBool(true));

    _cameraCalc.valueSetIsDistance()->setRawValue   (complexObject[_jsonV3FixedValueIsAltitudeKey].toBool(true));
    _cameraCalc.setDistanceToSurfaceRelative        (complexObject[_jsonV3GridAltitudeRelativeKey].toBool(true));
Don Gagne's avatar
Don Gagne committed
244

245
    bool manualGrid = complexObject[_jsonV3ManualGridKey].toBool(true);
246

Don Gagne's avatar
Don Gagne committed
247
    QList<JsonHelper::KeyValidateInfo> gridKeyInfoList = {
248 249 250 251
        { _jsonV3GridAltitudeKey,           QJsonValue::Double, true },
        { _jsonV3GridAltitudeRelativeKey,   QJsonValue::Bool,   true },
        { _jsonV3GridAngleKey,              QJsonValue::Double, true },
        { _jsonV3GridSpacingKey,            QJsonValue::Double, true },
DonLakeFlyer's avatar
DonLakeFlyer committed
252
        { _jsonEntryPointKey,      QJsonValue::Double, false },
253
        { _jsonV3TurnaroundDistKey,         QJsonValue::Double, true },
Don Gagne's avatar
Don Gagne committed
254
    };
255
    QJsonObject gridObject = complexObject[_jsonV3GridObjectKey].toObject();
Don Gagne's avatar
Don Gagne committed
256
    if (!JsonHelper::validateKeys(gridObject, gridKeyInfoList, errorString)) {
257
        _ignoreRecalc = false;
Don Gagne's avatar
Don Gagne committed
258 259
        return false;
    }
260 261 262 263

    _gridAngleFact.setRawValue          (gridObject[_jsonV3GridAngleKey].toDouble());
    _turnAroundDistanceFact.setRawValue (gridObject[_jsonV3TurnaroundDistKey].toDouble());

DonLakeFlyer's avatar
DonLakeFlyer committed
264 265
    if (gridObject.contains(_jsonEntryPointKey)) {
        _entryPoint = gridObject[_jsonEntryPointKey].toDouble();
DonLakeFlyer's avatar
DonLakeFlyer committed
266
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
267
        _entryPoint = EntryLocationTopRight;
DonLakeFlyer's avatar
DonLakeFlyer committed
268
    }
Don Gagne's avatar
Don Gagne committed
269

270 271 272 273 274
    _cameraCalc.distanceToSurface()->setRawValue        (gridObject[_jsonV3GridAltitudeKey].toDouble());
    _cameraCalc.adjustedFootprintSide()->setRawValue    (gridObject[_jsonV3GridSpacingKey].toDouble());
    _cameraCalc.adjustedFootprintFrontal()->setRawValue (complexObject[_jsonV3CameraTriggerDistanceKey].toDouble());

    if (manualGrid) {
275
        _cameraCalc.cameraName()->setRawValue(_cameraCalc.manualCameraName());
276 277
    } else {
        if (!complexObject.contains(_jsonV3CameraObjectKey)) {
Don Gagne's avatar
Don Gagne committed
278
            errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera");
279
            _ignoreRecalc = false;
Don Gagne's avatar
Don Gagne committed
280 281 282
            return false;
        }

283
        QJsonObject cameraObject = complexObject[_jsonV3CameraObjectKey].toObject();
Don Gagne's avatar
Don Gagne committed
284

285 286 287
        // Older code had typo on "imageSideOverlap" incorrectly being "imageSizeOverlap"
        QString incorrectImageSideOverlap = "imageSizeOverlap";
        if (cameraObject.contains(incorrectImageSideOverlap)) {
288
            cameraObject[_jsonV3SideOverlapKey] = cameraObject[incorrectImageSideOverlap];
289 290 291
            cameraObject.remove(incorrectImageSideOverlap);
        }

Don Gagne's avatar
Don Gagne committed
292
        QList<JsonHelper::KeyValidateInfo> cameraKeyInfoList = {
293 294 295 296 297 298 299 300 301 302 303
            { _jsonV3GroundResolutionKey,           QJsonValue::Double, true },
            { _jsonV3FrontalOverlapKey,             QJsonValue::Double, true },
            { _jsonV3SideOverlapKey,                QJsonValue::Double, true },
            { _jsonV3CameraSensorWidthKey,          QJsonValue::Double, true },
            { _jsonV3CameraSensorHeightKey,         QJsonValue::Double, true },
            { _jsonV3CameraResolutionWidthKey,      QJsonValue::Double, true },
            { _jsonV3CameraResolutionHeightKey,     QJsonValue::Double, true },
            { _jsonV3CameraFocalLengthKey,          QJsonValue::Double, true },
            { _jsonV3CameraNameKey,                 QJsonValue::String, true },
            { _jsonV3CameraOrientationLandscapeKey, QJsonValue::Bool,   true },
            { _jsonV3CameraMinTriggerIntervalKey,   QJsonValue::Double, false },
Don Gagne's avatar
Don Gagne committed
304
        };
Don Gagne's avatar
Don Gagne committed
305
        if (!JsonHelper::validateKeys(cameraObject, cameraKeyInfoList, errorString)) {
306
            _ignoreRecalc = false;
Don Gagne's avatar
Don Gagne committed
307 308 309
            return false;
        }

310
        _cameraCalc.cameraName()->setRawValue           (cameraObject[_jsonV3CameraNameKey].toString());
311 312 313 314 315 316 317 318 319 320 321
        _cameraCalc.landscape()->setRawValue            (cameraObject[_jsonV3CameraOrientationLandscapeKey].toBool(true));
        _cameraCalc.frontalOverlap()->setRawValue       (cameraObject[_jsonV3FrontalOverlapKey].toInt());
        _cameraCalc.sideOverlap()->setRawValue          (cameraObject[_jsonV3SideOverlapKey].toInt());
        _cameraCalc.sensorWidth()->setRawValue          (cameraObject[_jsonV3CameraSensorWidthKey].toDouble());
        _cameraCalc.sensorHeight()->setRawValue         (cameraObject[_jsonV3CameraSensorHeightKey].toDouble());
        _cameraCalc.focalLength()->setRawValue          (cameraObject[_jsonV3CameraFocalLengthKey].toDouble());
        _cameraCalc.imageWidth()->setRawValue           (cameraObject[_jsonV3CameraResolutionWidthKey].toInt());
        _cameraCalc.imageHeight()->setRawValue          (cameraObject[_jsonV3CameraResolutionHeightKey].toInt());
        _cameraCalc.minTriggerInterval()->setRawValue   (cameraObject[_jsonV3CameraMinTriggerIntervalKey].toDouble(0));
        _cameraCalc.imageDensity()->setRawValue         (cameraObject[_jsonV3GroundResolutionKey].toDouble());
        _cameraCalc.fixedOrientation()->setRawValue     (false);
Don Gagne's avatar
Don Gagne committed
322
    }
323 324

    // Polygon shape
325 326 327 328 329
    /// Load a polygon from json
    ///     @param json Json object to load from
    ///     @param required true: no polygon in object will generate error
    ///     @param errorString Error string if return is false
    /// @return true: success, false: failure (errorString set)
330 331 332
    if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) {
        _surveyAreaPolygon.clear();
        _ignoreRecalc = false;
333 334
        return false;
    }
335

336 337
    _ignoreRecalc = false;

338 339 340
    return true;
}

DonLakeFlyer's avatar
DonLakeFlyer committed
341
/// Reverse the order of the transects. First transect becomes last and so forth.
342
void SurveyComplexItem::_reverseTransectOrder(QList<QList<QGeoCoordinate>>& transects)
DonLakeFlyer's avatar
DonLakeFlyer committed
343 344 345 346 347 348 349 350 351
{
    QList<QList<QGeoCoordinate>> rgReversedTransects;
    for (int i=transects.count() - 1; i>=0; i--) {
        rgReversedTransects.append(transects[i]);
    }
    transects = rgReversedTransects;
}

/// Reverse the order of all points withing each transect, First point becomes last and so forth.
352
void SurveyComplexItem::_reverseInternalTransectPoints(QList<QList<QGeoCoordinate>>& transects)
DonLakeFlyer's avatar
DonLakeFlyer committed
353 354 355 356 357 358 359 360 361 362 363
{
    for (int i=0; i<transects.count(); i++) {
        QList<QGeoCoordinate> rgReversedCoords;
        QList<QGeoCoordinate>& rgOriginalCoords = transects[i];
        for (int j=rgOriginalCoords.count()-1; j>=0; j--) {
            rgReversedCoords.append(rgOriginalCoords[j]);
        }
        transects[i] = rgReversedCoords;
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
364 365 366 367
/// Reorders the transects such that the first transect is the shortest distance to the specified coordinate
/// and the first point within that transect is the shortest distance to the specified coordinate.
///     @param distanceCoord Coordinate to measure distance against
///     @param transects Transects to test and reorder
368
void SurveyComplexItem::_optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList<QList<QGeoCoordinate>>& transects)
369
{
370
    double rgTransectDistance[4];
DonLakeFlyer's avatar
DonLakeFlyer committed
371 372 373 374
    rgTransectDistance[0] = transects.first().first().distanceTo(distanceCoord);
    rgTransectDistance[1] = transects.first().last().distanceTo(distanceCoord);
    rgTransectDistance[2] = transects.last().first().distanceTo(distanceCoord);
    rgTransectDistance[3] = transects.last().last().distanceTo(distanceCoord);
375 376 377 378 379 380 381 382 383

    int shortestIndex = 0;
    double shortestDistance = rgTransectDistance[0];
    for (int i=1; i<3; i++) {
        if (rgTransectDistance[i] < shortestDistance) {
            shortestIndex = i;
            shortestDistance = rgTransectDistance[i];
        }
    }
384

385 386
    if (shortestIndex > 1) {
        // We need to reverse the order of segments
DonLakeFlyer's avatar
DonLakeFlyer committed
387
        _reverseTransectOrder(transects);
388 389 390
    }
    if (shortestIndex & 1) {
        // We need to reverse the points within each segment
DonLakeFlyer's avatar
DonLakeFlyer committed
391
        _reverseInternalTransectPoints(transects);
392 393
    }
}
394

395
qreal SurveyComplexItem::_ccw(QPointF pt1, QPointF pt2, QPointF pt3)
396 397 398 399
{
    return (pt2.x()-pt1.x())*(pt3.y()-pt1.y()) - (pt2.y()-pt1.y())*(pt3.x()-pt1.x());
}

400
qreal SurveyComplexItem::_dp(QPointF pt1, QPointF pt2)
401 402 403 404
{
    return (pt2.x()-pt1.x())/qSqrt((pt2.x()-pt1.x())*(pt2.x()-pt1.x()) + (pt2.y()-pt1.y())*(pt2.y()-pt1.y()));
}

405
void SurveyComplexItem::_swapPoints(QList<QPointF>& points, int index1, int index2)
406 407 408 409 410 411
{
    QPointF temp = points[index1];
    points[index1] = points[index2];
    points[index2] = temp;
}

DonLakeFlyer's avatar
DonLakeFlyer committed
412
/// Returns true if the current grid angle generates north/south oriented transects
413
bool SurveyComplexItem::_gridAngleIsNorthSouthTransects()
DonLakeFlyer's avatar
DonLakeFlyer committed
414 415 416 417 418 419
{
    // Grid angle ranges from -360<->360
    double gridAngle = qAbs(_gridAngleFact.rawValue().toDouble());
    return gridAngle < 45.0 || (gridAngle > 360.0 - 45.0) || (gridAngle > 90.0 + 45.0 && gridAngle < 270.0 - 45.0);
}

420
void SurveyComplexItem::_adjustTransectsToEntryPointLocation(QList<QList<QGeoCoordinate>>& transects)
DonLakeFlyer's avatar
DonLakeFlyer committed
421 422 423 424 425
{
    if (transects.count() == 0) {
        return;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
426 427
    bool reversePoints = false;
    bool reverseTransects = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
428

DonLakeFlyer's avatar
DonLakeFlyer committed
429
    if (_entryPoint == EntryLocationBottomLeft || _entryPoint == EntryLocationBottomRight) {
DonLakeFlyer's avatar
DonLakeFlyer committed
430
        reversePoints = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
431
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
432
    if (_entryPoint == EntryLocationTopRight || _entryPoint == EntryLocationBottomRight) {
DonLakeFlyer's avatar
DonLakeFlyer committed
433
        reverseTransects = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
434
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
435

DonLakeFlyer's avatar
DonLakeFlyer committed
436
    if (reversePoints) {
437
        qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Reverse Points";
DonLakeFlyer's avatar
DonLakeFlyer committed
438 439 440
        _reverseInternalTransectPoints(transects);
    }
    if (reverseTransects) {
441
        qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Reverse Transects";
DonLakeFlyer's avatar
DonLakeFlyer committed
442
        _reverseTransectOrder(transects);
DonLakeFlyer's avatar
DonLakeFlyer committed
443
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
444

DonLakeFlyer's avatar
DonLakeFlyer committed
445
    qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Modified entry point:entryLocation" << transects.first().first() << _entryPoint;
DonLakeFlyer's avatar
DonLakeFlyer committed
446 447
}

448
QPointF SurveyComplexItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
449 450
{
    QPointF rotated;
DonLakeFlyer's avatar
DonLakeFlyer committed
451
    double radians = (M_PI / 180.0) * -angle;
452 453 454 455 456 457 458

    rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x());
    rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y());

    return rotated;
}

459
void SurveyComplexItem::_intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines)
460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515
{
    QLineF topLine      (boundRect.topLeft(),       boundRect.topRight());
    QLineF bottomLine   (boundRect.bottomLeft(),    boundRect.bottomRight());
    QLineF leftLine     (boundRect.topLeft(),       boundRect.bottomLeft());
    QLineF rightLine    (boundRect.topRight(),      boundRect.bottomRight());

    for (int i=0; i<lineList.count(); i++) {
        QPointF intersectPoint;
        QLineF intersectLine;
        const QLineF& line = lineList[i];

        int foundCount = 0;
        if (line.intersect(topLine, &intersectPoint) == QLineF::BoundedIntersection) {
            intersectLine.setP1(intersectPoint);
            foundCount++;
        }
        if (line.intersect(rightLine, &intersectPoint) == QLineF::BoundedIntersection) {
            if (foundCount == 0) {
                intersectLine.setP1(intersectPoint);
            } else {
                if (foundCount != 1) {
                    qWarning() << "Found more than two intersecting points";
                }
                intersectLine.setP2(intersectPoint);
            }
            foundCount++;
        }
        if (line.intersect(bottomLine, &intersectPoint) == QLineF::BoundedIntersection) {
            if (foundCount == 0) {
                intersectLine.setP1(intersectPoint);
            } else {
                if (foundCount != 1) {
                    qWarning() << "Found more than two intersecting points";
                }
                intersectLine.setP2(intersectPoint);
            }
            foundCount++;
        }
        if (line.intersect(leftLine, &intersectPoint) == QLineF::BoundedIntersection) {
            if (foundCount == 0) {
                intersectLine.setP1(intersectPoint);
            } else {
                if (foundCount != 1) {
                    qWarning() << "Found more than two intersecting points";
                }
                intersectLine.setP2(intersectPoint);
            }
            foundCount++;
        }

        if (foundCount == 2) {
            resultLines += intersectLine;
        }
    }
}

516
void SurveyComplexItem::_intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines)
517
{
518
    resultLines.clear();
519

520 521
    for (int i=0; i<lineList.count(); i++) {
        const QLineF& line = lineList[i];
522
        QList<QPointF> intersections;
523

524
        // Intersect the line with all the polygon edges
525 526 527 528
        for (int j=0; j<polygon.count()-1; j++) {
            QPointF intersectPoint;
            QLineF polygonLine = QLineF(polygon[j], polygon[j+1]);
            if (line.intersect(polygonLine, &intersectPoint) == QLineF::BoundedIntersection) {
529 530 531
                if (!intersections.contains(intersectPoint)) {
                    intersections.append(intersectPoint);
                }
532 533 534
            }
        }

535 536 537 538 539 540 541 542 543 544
        // We now have one or more intersection points all along the same line. Find the two
        // which are furthest away from each other to form the transect.
        if (intersections.count() > 1) {
            QPointF firstPoint;
            QPointF secondPoint;
            double currentMaxDistance = 0;

            for (int i=0; i<intersections.count(); i++) {
                for (int j=0; j<intersections.count(); j++) {
                    QLineF lineTest(intersections[i], intersections[j]);
545
                    \
546 547 548 549 550 551 552 553 554 555
                    double newMaxDistance = lineTest.length();
                    if (newMaxDistance > currentMaxDistance) {
                        firstPoint = intersections[i];
                        secondPoint = intersections[j];
                        currentMaxDistance = newMaxDistance;
                    }
                }
            }

            resultLines += QLineF(firstPoint, secondPoint);
556 557 558 559 560
        }
    }
}

/// Adjust the line segments such that they are all going the same direction with respect to going from P1->P2
561
void SurveyComplexItem::_adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines)
562
{
563
    qreal firstAngle = 0;
564 565 566 567
    for (int i=0; i<lineList.count(); i++) {
        const QLineF& line = lineList[i];
        QLineF adjustedLine;

568 569 570 571 572
        if (i == 0) {
            firstAngle = line.angle();
        }

        if (qAbs(line.angle() - firstAngle) > 1.0) {
573 574 575 576 577 578 579 580 581 582
            adjustedLine.setP1(line.p2());
            adjustedLine.setP2(line.p1());
        } else {
            adjustedLine = line;
        }

        resultLines += adjustedLine;
    }
}

583
double SurveyComplexItem::_clampGridAngle90(double gridAngle)
DonLakeFlyer's avatar
DonLakeFlyer committed
584 585 586 587 588 589 590 591 592 593
{
    // Clamp grid angle to -90<->90. This prevents transects from being rotated to a reversed order.
    if (gridAngle > 90.0) {
        gridAngle -= 180.0;
    } else if (gridAngle < -90.0) {
        gridAngle += 180;
    }
    return gridAngle;
}

594
#if 0
595
int SurveyComplexItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QList<QList<QPointF>>& transectSegments, bool refly)
596
{
597 598
    int cameraShots = 0;

DonLakeFlyer's avatar
DonLakeFlyer committed
599
    double gridAngle = _gridAngleFact.rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
600 601
    double gridSpacing = _gridSpacingFact.rawValue().toDouble();

DonLakeFlyer's avatar
DonLakeFlyer committed
602 603
    gridAngle = _clampGridAngle90(gridAngle);
    gridAngle += refly ? 90 : 0;
604
    qCDebug(SurveyComplexItemLog) << "Clamped grid angle" << gridAngle;
DonLakeFlyer's avatar
DonLakeFlyer committed
605

606
    qCDebug(SurveyComplexItemLog) << "SurveyComplexItem::_gridGenerator gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly;
607

DonLakeFlyer's avatar
DonLakeFlyer committed
608
    transectSegments.clear();
609 610 611

    // Convert polygon to bounding rect

612
    qCDebug(SurveyComplexItemLog) << "Polygon";
613 614
    QPolygonF polygon;
    for (int i=0; i<polygonPoints.count(); i++) {
615
        qCDebug(SurveyComplexItemLog) << polygonPoints[i];
616 617 618
        polygon << polygonPoints[i];
    }
    polygon << polygonPoints[0];
619 620
    QRectF boundingRect = polygon.boundingRect();
    QPointF boundingCenter = boundingRect.center();
621
    qCDebug(SurveyComplexItemLog) << "Bounding rect" << boundingRect.topLeft().x() << boundingRect.topLeft().y() << boundingRect.bottomRight().x() << boundingRect.bottomRight().y();
622 623 624

    // Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
    // bounding box to guarantee intersection.
DonLakeFlyer's avatar
DonLakeFlyer committed
625

626
    QList<QLineF> lineList;
DonLakeFlyer's avatar
DonLakeFlyer committed
627

628
    // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor.
DonLakeFlyer's avatar
DonLakeFlyer committed
629
    // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to.
630
    // They are initially generated with the transects flowing from west to east and then points within the transect north to south.
631
    double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0;
632 633 634 635 636 637 638 639 640 641 642
    double halfWidth = maxWidth / 2.0;
    double transectX = boundingCenter.x() - halfWidth;
    double transectXMax = transectX + maxWidth;
    while (transectX < transectXMax) {
        double transectYTop = boundingCenter.y() - halfWidth;
        double transectYBottom = boundingCenter.y() + halfWidth;

        lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle));
        transectX += gridSpacing;
    }

643 644 645 646 647 648 649 650 651
    // Now intersect the lines with the polygon
    QList<QLineF> intersectLines;
#if 1
    _intersectLinesWithPolygon(lineList, polygon, intersectLines);
#else
    // This is handy for debugging grid problems, not for release
    intersectLines = lineList;
#endif

652 653 654 655 656 657 658 659 660 661 662 663 664 665 666
    // Less than two transects intersected with the polygon:
    //      Create a single transect which goes through the center of the polygon
    //      Intersect it with the polygon
    if (intersectLines.count() < 2) {
        _mapPolygon.center();
        QLineF firstLine = lineList.first();
        QPointF lineCenter = firstLine.pointAt(0.5);
        QPointF centerOffset = boundingCenter - lineCenter;
        firstLine.translate(centerOffset);
        lineList.clear();
        lineList.append(firstLine);
        intersectLines = lineList;
        _intersectLinesWithPolygon(lineList, polygon, intersectLines);
    }

667 668 669 670 671
    // Make sure all lines are going to same direction. Polygon intersection leads to line which
    // can be in varied directions depending on the order of the intesecting sides.
    QList<QLineF> resultLines;
    _adjustLineDirection(intersectLines, resultLines);

672
    // Calc camera shots here if there are no images in turnaround
DonLakeFlyer's avatar
DonLakeFlyer committed
673
    if (_triggerCamera() && !_imagesEverywhere()) {
674
        for (int i=0; i<resultLines.count(); i++) {
675 676 677
            cameraShots += (int)floor(resultLines[i].length() / _triggerDistance());
            // Take into account immediate camera trigger at waypoint entry
            cameraShots++;
678 679 680 681
        }
    }

    // Turn into a path
682
    for (int i=0; i<resultLines.count(); i++) {
DonLakeFlyer's avatar
DonLakeFlyer committed
683 684 685
        QLineF          transectLine;
        QList<QPointF>  transectPoints;
        const QLineF&   line = resultLines[i];
686

DonLakeFlyer's avatar
DonLakeFlyer committed
687
        float turnaroundPosition = _turnaroundDistance() / line.length();
Andreas Bircher's avatar
Andreas Bircher committed
688

689
        if (i & 1) {
DonLakeFlyer's avatar
DonLakeFlyer committed
690
            transectLine = QLineF(line.p2(), line.p1());
691
        } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707
            transectLine = QLineF(line.p1(), line.p2());
        }

        // Build the points along the transect

        if (_hasTurnaround()) {
            transectPoints.append(transectLine.pointAt(-turnaroundPosition));
        }

        // Polygon entry point
        transectPoints.append(transectLine.p1());

        // For hover and capture we need points for each camera location
        if (_triggerCamera() && _hoverAndCaptureEnabled()) {
            if (_triggerDistance() < transectLine.length()) {
                int innerPoints = floor(transectLine.length() / _triggerDistance());
708
                qCDebug(SurveyComplexItemLog) << "innerPoints" << innerPoints;
DonLakeFlyer's avatar
DonLakeFlyer committed
709 710 711 712
                float transectPositionIncrement = _triggerDistance() / transectLine.length();
                for (int i=0; i<innerPoints; i++) {
                    transectPoints.append(transectLine.pointAt(transectPositionIncrement * (i + 1)));
                }
Andreas Bircher's avatar
Andreas Bircher committed
713
            }
714
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
715 716 717 718 719 720 721 722 723

        // Polygon exit point
        transectPoints.append(transectLine.p2());

        if (_hasTurnaround()) {
            transectPoints.append(transectLine.pointAt(1 + turnaroundPosition));
        }

        transectSegments.append(transectPoints);
724
    }
725 726

    return cameraShots;
727
}
728
#endif
729

730
#if 0
731
int SurveyComplexItem::_appendWaypointToMission(QList<MissionItem*>& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent)
732
{
DonLakeFlyer's avatar
DonLakeFlyer committed
733 734 735
    double  altitude =          _gridAltitudeFact.rawValue().toDouble();
    bool    altitudeRelative =  _gridAltitudeRelativeFact.rawValue().toBool();

736
    qCDebug(SurveyComplexItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone);
737 738 739 740

    MissionItem* item = new MissionItem(seqNum++,
                                        MAV_CMD_NAV_WAYPOINT,
                                        altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
741
                                        cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0,  // Hold time (delay for hover and capture to settle vehicle before image is taken)
742 743
                                        0.0, 0.0,
                                        std::numeric_limits<double>::quiet_NaN(),   // Yaw unchanged
744 745 746
                                        coord.latitude(),
                                        coord.longitude(),
                                        altitude,
747 748
                                        true,                                       // autoContinue
                                        false,                                      // isCurrentItem
749 750 751
                                        missionItemParent);
    items.append(item);

DonLakeFlyer's avatar
DonLakeFlyer committed
752 753 754 755 756 757 758
    switch (cameraTrigger) {
    case CameraTriggerOff:
    case CameraTriggerOn:
        item = new MissionItem(seqNum++,
                               MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                               MAV_FRAME_MISSION,
                               cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0,
759 760 761 762 763
                               0,                                           // shutter integration (ignore)
                               cameraTrigger == CameraTriggerOn ? 1 : 0,    // trigger immediately when starting
                               0, 0, 0, 0,                                  // param 4-7 unused
                               true,                                        // autoContinue
                               false,                                       // isCurrentItem
DonLakeFlyer's avatar
DonLakeFlyer committed
764
                               missionItemParent);
765
        items.append(item);
DonLakeFlyer's avatar
DonLakeFlyer committed
766 767 768 769 770
        break;
    case CameraTriggerHoverAndCapture:
        item = new MissionItem(seqNum++,
                               MAV_CMD_IMAGE_START_CAPTURE,
                               MAV_FRAME_MISSION,
Gus Grubba's avatar
Gus Grubba committed
771
                               0,                           // Reserved (Set to 0)
772 773
                               0,                           // Interval (none)
                               1,                           // Take 1 photo
774
                               NAN, NAN, NAN, NAN,          // param 4-7 reserved
775 776
                               true,                        // autoContinue
                               false,                       // isCurrentItem
DonLakeFlyer's avatar
DonLakeFlyer committed
777 778
                               missionItemParent);
        items.append(item);
779 780
#if 0
        // This generates too many commands. Pulling out for now, to see if image quality is still high enough.
DonLakeFlyer's avatar
DonLakeFlyer committed
781 782 783 784 785 786 787 788 789 790
        item = new MissionItem(seqNum++,
                               MAV_CMD_NAV_DELAY,
                               MAV_FRAME_MISSION,
                               0.5,                // Delay in seconds, give some time for image to be taken
                               -1, -1, -1,         // No time
                               0, 0, 0,            // Param 5-7 unused
                               true,               // autoContinue
                               false,              // isCurrentItem
                               missionItemParent);
        items.append(item);
791
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
792 793
    default:
        break;
794
    }
795 796

    return seqNum;
797
}
798
#endif
799

800
bool SurveyComplexItem::_nextTransectCoord(const QList<QGeoCoordinate>& transectPoints, int pointIndex, QGeoCoordinate& coord)
DonLakeFlyer's avatar
DonLakeFlyer committed
801 802 803 804 805 806 807 808 809 810
{
    if (pointIndex > transectPoints.count()) {
        qWarning() << "Bad grid generation";
        return false;
    }

    coord = transectPoints[pointIndex];
    return true;
}

811 812 813 814 815 816 817 818 819
void SurveyComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems";

    // Now build the mission items from the transect points

    MissionItem* item;
    int seqNum =                    _sequenceNumber;
    bool imagesEverywhere =         _cameraTriggerInTurnAroundFact.rawValue().toBool();
DonLakeFlyer's avatar
DonLakeFlyer committed
820
    bool addTriggerAtBeginning =    !hoverAndCaptureEnabled() && imagesEverywhere;
821 822 823 824
    bool firstOverallPoint =        true;

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

825
    for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
826 827
        bool entryPoint = true;

828
        for (const CoordInfo_t& transectCoordInfo: transect) {
829 830 831
            item = new MissionItem(seqNum++,
                                   MAV_CMD_NAV_WAYPOINT,
                                   mavFrame,
DonLakeFlyer's avatar
DonLakeFlyer committed
832 833 834 835 836
                                   hoverAndCaptureEnabled() ?
                                       _hoverAndCaptureDelaySeconds : 0,        // Hold time (delay for hover and capture to settle vehicle before image is taken)
                                   0.0,                                         // No acceptance radius specified
                                   0.0,                                         // Pass through waypoint
                                   std::numeric_limits<double>::quiet_NaN(),    // Yaw unchanged
837 838 839
                                   transectCoordInfo.coord.latitude(),
                                   transectCoordInfo.coord.longitude(),
                                   transectCoordInfo.coord.altitude(),
DonLakeFlyer's avatar
DonLakeFlyer committed
840 841
                                   true,                                        // autoContinue
                                   false,                                       // isCurrentItem
842 843
                                   missionItemParent);
            items.append(item);
DonLakeFlyer's avatar
DonLakeFlyer committed
844 845 846 847 848 849 850 851 852 853 854 855 856
            if (hoverAndCaptureEnabled()) {
                item = new MissionItem(seqNum++,
                                       MAV_CMD_IMAGE_START_CAPTURE,
                                       MAV_FRAME_MISSION,
                                       0,                           // Reserved (Set to 0)
                                       0,                           // Interval (none)
                                       1,                           // Take 1 photo
                                       NAN, NAN, NAN, NAN,          // param 4-7 reserved
                                       true,                        // autoContinue
                                       false,                       // isCurrentItem
                                       missionItemParent);
                items.append(item);
            }
857 858 859 860 861 862 863

            if (firstOverallPoint && addTriggerAtBeginning) {
                // Start triggering
                addTriggerAtBeginning = false;
                item = new MissionItem(seqNum++,
                                       MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                       MAV_FRAME_MISSION,
DonLakeFlyer's avatar
DonLakeFlyer committed
864 865 866 867 868 869
                                       triggerDistance(),   // trigger distance
                                       0,                   // shutter integration (ignore)
                                       1,                   // trigger immediately when starting
                                       0, 0, 0, 0,          // param 4-7 unused
                                       true,                // autoContinue
                                       false,               // isCurrentItem
870 871 872 873 874
                                       missionItemParent);
                items.append(item);
            }
            firstOverallPoint = false;

DonLakeFlyer's avatar
DonLakeFlyer committed
875
            if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && triggerCamera() && !hoverAndCaptureEnabled() && !imagesEverywhere) {
876 877 878 879 880
                if (entryPoint) {
                    // Start of transect, start triggering
                    item = new MissionItem(seqNum++,
                                           MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                           MAV_FRAME_MISSION,
DonLakeFlyer's avatar
DonLakeFlyer committed
881 882 883 884 885 886
                                           triggerDistance(),   // trigger distance
                                           0,                   // shutter integration (ignore)
                                           1,                   // trigger immediately when starting
                                           0, 0, 0, 0,          // param 4-7 unused
                                           true,                // autoContinue
                                           false,               // isCurrentItem
887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907
                                           missionItemParent);
                    items.append(item);
                } else {
                    // End of transect, stop triggering
                    item = new MissionItem(seqNum++,
                                           MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                           MAV_FRAME_MISSION,
                                           0,           // stop triggering
                                           0,           // shutter integration (ignore)
                                           0,           // trigger immediately when starting
                                           0, 0, 0, 0,  // param 4-7 unused
                                           true,        // autoContinue
                                           false,       // isCurrentItem
                                           missionItemParent);
                    items.append(item);
                }
                entryPoint = !entryPoint;
            }
        }
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
908
    if (triggerCamera() && !hoverAndCaptureEnabled() &&  imagesEverywhere) {
909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925
        // Stop triggering
        MissionItem* item = new MissionItem(seqNum++,
                                            MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                            MAV_FRAME_MISSION,
                                            0,           // stop triggering
                                            0,           // shutter integration (ignore)
                                            0,           // trigger immediately when starting
                                            0, 0, 0, 0,  // param 4-7 unused
                                            true,        // autoContinue
                                            false,       // isCurrentItem
                                            missionItemParent);
        items.append(item);
    }
}

#if 0
bool SurveyComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
926
{
927
    int seqNum = _sequenceNumber;
928 929
    bool firstWaypointTrigger = false;

930 931 932 933 934 935
#if 1
    // FIXME: Hack
    bool hasRefly = false;
    bool buildRefly = false;
#endif

936
    qCDebug(SurveyComplexItemLog) << QStringLiteral("hasTurnaround(%1) triggerCamera(%2) hoverAndCapture(%3) imagesEverywhere(%4) hasRefly(%5) buildRefly(%6) ").arg(_hasTurnaround()).arg(_triggerCamera()).arg(_hoverAndCaptureEnabled()).arg(_imagesEverywhere()).arg(hasRefly).arg(buildRefly);
937

938
#if 0
939
    QList<QList<QGeoCoordinate>>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments;
940
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
941

942
    if (!buildRefly && _imagesEverywhere()) {
943
        firstWaypointTrigger = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
944 945
    }

946
    for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
DonLakeFlyer's avatar
DonLakeFlyer committed
947 948
        int pointIndex = 0;
        QGeoCoordinate coord;
949
        CameraTriggerCode cameraTrigger;
DonLakeFlyer's avatar
DonLakeFlyer committed
950

951
        qCDebug(SurveyComplexItemLog) << "transect.count" << transect.count();
DonLakeFlyer's avatar
DonLakeFlyer committed
952 953 954

        if (_hasTurnaround()) {
            // Add entry turnaround point
955 956
            if (!_nextTransectCoord(segment, pointIndex++, coord)) {
                return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
957
            }
958 959
            seqNum = _appendWaypointToMission(items, seqNum, coord, firstWaypointTrigger ? CameraTriggerOn : CameraTriggerNone, missionItemParent);
            firstWaypointTrigger = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
960
        }
961

DonLakeFlyer's avatar
DonLakeFlyer committed
962
        // Add polygon entry point
963 964
        if (!_nextTransectCoord(segment, pointIndex++, coord)) {
            return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
965
        }
966 967 968 969 970
        if (firstWaypointTrigger) {
            cameraTrigger = CameraTriggerOn;
        } else {
            cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn);
        }
971
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);
972
        firstWaypointTrigger = false;
973

DonLakeFlyer's avatar
DonLakeFlyer committed
974 975
        // Add internal hover and capture points
        if (_hoverAndCaptureEnabled()) {
976
            int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0);
977
            qCDebug(SurveyComplexItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex;
DonLakeFlyer's avatar
DonLakeFlyer committed
978
            for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) {
979 980
                if (!_nextTransectCoord(segment, pointIndex, coord)) {
                    return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
981 982
                }
                seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent);
983 984
            }
        }
985

DonLakeFlyer's avatar
DonLakeFlyer committed
986
        // Add polygon exit point
987 988
        if (!_nextTransectCoord(segment, pointIndex++, coord)) {
            return false;
989
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
990
        cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff);
991 992
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);

DonLakeFlyer's avatar
DonLakeFlyer committed
993 994
        if (_hasTurnaround()) {
            // Add exit turnaround point
995 996
            if (!_nextTransectCoord(segment, pointIndex++, coord)) {
                return false;
997
            }
DonLakeFlyer's avatar
DonLakeFlyer committed
998
            seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent);
999
        }
1000

1001
        qCDebug(SurveyComplexItemLog) << "last PointIndex" << pointIndex;
1002 1003
    }

1004
    if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) {
1005 1006 1007 1008 1009 1010 1011 1012
        // Turn off camera at end of survey
        MissionItem* item = new MissionItem(seqNum++,
                                            MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                            MAV_FRAME_MISSION,
                                            0.0,                    // trigger distance (off)
                                            0, 0, 0, 0, 0, 0,       // param 2-7 unused
                                            true,                   // autoContinue
                                            false,                  // isCurrentItem
1013 1014
                                            missionItemParent);
        items.append(item);
1015
    }
1016 1017 1018

    return true;
}
1019
#endif
1020

1021
#if 0
1022
void SurveyComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
1023 1024
{
    int seqNum = _sequenceNumber;
1025
    bool refly = refly90Degrees()->rawValue().toBool();
1026

1027
    if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, refly, false /* buildRefly */)) {
1028 1029 1030
        return;
    }

1031 1032
    if (refly) {
        _appendMissionItemsWorker(items, missionItemParent, seqNum, refly, true /* buildRefly */);
1033 1034
    }
}
1035
#endif
1036

1037
bool SurveyComplexItem::_hasTurnaround(void) const
1038
{
1039
    return _turnaroundDistance() > 0;
1040
}
DonLakeFlyer's avatar
DonLakeFlyer committed
1041

1042
double SurveyComplexItem::_turnaroundDistance(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
1043
{
1044
    return _turnAroundDistanceFact.rawValue().toDouble();
DonLakeFlyer's avatar
DonLakeFlyer committed
1045 1046
}

1047
#if 0
1048
bool SurveyComplexItem::_triggerCamera(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
1049
{
1050
    return _triggerDistance() > 0;
DonLakeFlyer's avatar
DonLakeFlyer committed
1051 1052
}

1053
bool SurveyComplexItem::_imagesEverywhere(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
1054 1055 1056 1057
{
    return _triggerCamera() && _cameraTriggerInTurnaroundFact.rawValue().toBool();
}

1058
bool SurveyComplexItem::_hoverAndCaptureEnabled(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
1059 1060 1061
{
    return hoverAndCaptureAllowed() && !_imagesEverywhere() && _triggerCamera() && _hoverAndCaptureFact.rawValue().toBool();
}
1062
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
1063

1064
void SurveyComplexItem::_rebuildTransectsPhase1(void)
1065
{
1066 1067 1068 1069 1070 1071
	bool split = qgcApp()->toolbox()->settingsManager()->appSettings()->splitConcavePolygons()->rawValue().toBool();
	if (split) {
		_rebuildTransectsPhase1WorkerSplitPolygons(false /* refly */);
	} else {
		_rebuildTransectsPhase1WorkerSinglePolygon(false /* refly */);
	}
1072
    if (_refly90DegreesFact.rawValue().toBool()) {
1073 1074 1075 1076 1077
    	if (split) {
    		_rebuildTransectsPhase1WorkerSplitPolygons(true /* refly */);
    	} else {
    		_rebuildTransectsPhase1WorkerSinglePolygon(true /* refly */);
    	}
1078 1079 1080
    }
}

1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297
void SurveyComplexItem::_rebuildTransectsPhase1WorkerSinglePolygon(bool refly)
{
    if (_ignoreRecalc) {
        return;
    }

    // If the transects are getting rebuilt then any previously loaded mission items are now invalid
    if (_loadedMissionItemsParent) {
        _loadedMissionItems.clear();
        _loadedMissionItemsParent->deleteLater();
        _loadedMissionItemsParent = NULL;
    }

    // First pass will clear old transect data, refly will append to existing data
    if (!refly) {
        _transects.clear();
        _transectsPathHeightInfo.clear();
    }

    if (_surveyAreaPolygon.count() < 3) {
        return;
    }

    // Convert polygon to NED

    QList<QPointF> polygonPoints;
    QGeoCoordinate tangentOrigin = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(0)->coordinate();
    qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" << _surveyAreaPolygon.count() << tangentOrigin;
    for (int i=0; i<_surveyAreaPolygon.count(); i++) {
        double y, x, down;
        QGeoCoordinate vertex = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(i)->coordinate();
        if (i == 0) {
            // This avoids a nan calculation that comes out of convertGeoToNed
            x = y = 0;
        } else {
            convertGeoToNed(vertex, tangentOrigin, &y, &x, &down);
        }
        polygonPoints += QPointF(x, y);
        qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
    }

    // Generate transects

    double gridAngle = _gridAngleFact.rawValue().toDouble();
    double gridSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();

    gridAngle = _clampGridAngle90(gridAngle);
    gridAngle += refly ? 90 : 0;
    qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Clamped grid angle" << gridAngle;

    qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly;

    // Convert polygon to bounding rect

    qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Polygon";
    QPolygonF polygon;
    for (int i=0; i<polygonPoints.count(); i++) {
        qCDebug(SurveyComplexItemLog) << "Vertex" << polygonPoints[i];
        polygon << polygonPoints[i];
    }
    polygon << polygonPoints[0];
    QRectF boundingRect = polygon.boundingRect();
    QPointF boundingCenter = boundingRect.center();
    qCDebug(SurveyComplexItemLog) << "Bounding rect" << boundingRect.topLeft().x() << boundingRect.topLeft().y() << boundingRect.bottomRight().x() << boundingRect.bottomRight().y();

    // Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
    // bounding box to guarantee intersection.

    QList<QLineF> lineList;

    // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor.
    // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to.
    // They are initially generated with the transects flowing from west to east and then points within the transect north to south.
    double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0;
    double halfWidth = maxWidth / 2.0;
    double transectX = boundingCenter.x() - halfWidth;
    double transectXMax = transectX + maxWidth;
    while (transectX < transectXMax) {
        double transectYTop = boundingCenter.y() - halfWidth;
        double transectYBottom = boundingCenter.y() + halfWidth;

        lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle));
        transectX += gridSpacing;
    }

    // Now intersect the lines with the polygon
    QList<QLineF> intersectLines;
#if 1
    _intersectLinesWithPolygon(lineList, polygon, intersectLines);
#else
    // This is handy for debugging grid problems, not for release
    intersectLines = lineList;
#endif

    // Less than two transects intersected with the polygon:
    //      Create a single transect which goes through the center of the polygon
    //      Intersect it with the polygon
    if (intersectLines.count() < 2) {
        _surveyAreaPolygon.center();
        QLineF firstLine = lineList.first();
        QPointF lineCenter = firstLine.pointAt(0.5);
        QPointF centerOffset = boundingCenter - lineCenter;
        firstLine.translate(centerOffset);
        lineList.clear();
        lineList.append(firstLine);
        intersectLines = lineList;
        _intersectLinesWithPolygon(lineList, polygon, intersectLines);
    }

    // Make sure all lines are going the same direction. Polygon intersection leads to lines which
    // can be in varied directions depending on the order of the intesecting sides.
    QList<QLineF> resultLines;
    _adjustLineDirection(intersectLines, resultLines);

    // Convert from NED to Geo
    QList<QList<QGeoCoordinate>> transects;
    foreach (const QLineF& line, resultLines) {
        QGeoCoordinate          coord;
        QList<QGeoCoordinate>   transect;

        convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, &coord);
        transect.append(coord);
        convertNedToGeo(line.p2().y(), line.p2().x(), 0, tangentOrigin, &coord);
        transect.append(coord);

        transects.append(transect);
    }

    _adjustTransectsToEntryPointLocation(transects);

    if (refly) {
        _optimizeTransectsForShortestDistance(_transects.last().last().coord, transects);
    }

    if (_flyAlternateTransectsFact.rawValue().toBool()) {
        QList<QList<QGeoCoordinate>> alternatingTransects;
        for (int i=0; i<transects.count(); i++) {
            if (!(i & 1)) {
                alternatingTransects.append(transects[i]);
            }
        }
        for (int i=transects.count()-1; i>0; i--) {
            if (i & 1) {
                alternatingTransects.append(transects[i]);
            }
        }
        transects = alternatingTransects;
    }

    // Adjust to lawnmower pattern
    bool reverseVertices = false;
    for (int i=0; i<transects.count(); i++) {
        // We must reverse the vertices for every other transect in order to make a lawnmower pattern
        QList<QGeoCoordinate> transectVertices = transects[i];
        if (reverseVertices) {
            reverseVertices = false;
            QList<QGeoCoordinate> reversedVertices;
            for (int j=transectVertices.count()-1; j>=0; j--) {
                reversedVertices.append(transectVertices[j]);
            }
            transectVertices = reversedVertices;
        } else {
            reverseVertices = true;
        }
        transects[i] = transectVertices;
    }

    // Convert to CoordInfo transects and append to _transects
    foreach (const QList<QGeoCoordinate>& transect, transects) {
        QGeoCoordinate                                  coord;
        QList<TransectStyleComplexItem::CoordInfo_t>    coordInfoTransect;
        TransectStyleComplexItem::CoordInfo_t           coordInfo;

        coordInfo = { transect[0], CoordTypeSurveyEdge };
        coordInfoTransect.append(coordInfo);
        coordInfo = { transect[1], CoordTypeSurveyEdge };
        coordInfoTransect.append(coordInfo);

        // For hover and capture we need points for each camera location within the transect
        if (triggerCamera() && hoverAndCaptureEnabled()) {
            double transectLength = transect[0].distanceTo(transect[1]);
            double transectAzimuth = transect[0].azimuthTo(transect[1]);
            if (triggerDistance() < transectLength) {
                int cInnerHoverPoints = floor(transectLength / triggerDistance());
                qCDebug(SurveyComplexItemLog) << "cInnerHoverPoints" << cInnerHoverPoints;
                for (int i=0; i<cInnerHoverPoints; i++) {
                    QGeoCoordinate hoverCoord = transect[0].atDistanceAndAzimuth(triggerDistance() * (i + 1), transectAzimuth);
                    TransectStyleComplexItem::CoordInfo_t coordInfo = { hoverCoord, CoordTypeInteriorHoverTrigger };
                    coordInfoTransect.insert(1 + i, coordInfo);
                }
            }
        }

        // Extend the transect ends for turnaround
        if (_hasTurnaround()) {
            QGeoCoordinate turnaroundCoord;
            double turnAroundDistance = _turnAroundDistanceFact.rawValue().toDouble();

            double azimuth = transect[0].azimuthTo(transect[1]);
            turnaroundCoord = transect[0].atDistanceAndAzimuth(-turnAroundDistance, azimuth);
            turnaroundCoord.setAltitude(qQNaN());
            TransectStyleComplexItem::CoordInfo_t coordInfo = { turnaroundCoord, CoordTypeTurnaround };
            coordInfoTransect.prepend(coordInfo);

            azimuth = transect.last().azimuthTo(transect[transect.count() - 2]);
            turnaroundCoord = transect.last().atDistanceAndAzimuth(-turnAroundDistance, azimuth);
            turnaroundCoord.setAltitude(qQNaN());
            coordInfo = { turnaroundCoord, CoordTypeTurnaround };
            coordInfoTransect.append(coordInfo);
        }

        _transects.append(coordInfoTransect);
    }
}


void SurveyComplexItem::_rebuildTransectsPhase1WorkerSplitPolygons(bool refly)
DonLakeFlyer's avatar
DonLakeFlyer committed
1298
{
1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309
    if (_ignoreRecalc) {
        return;
    }

    // If the transects are getting rebuilt then any previously loaded mission items are now invalid
    if (_loadedMissionItemsParent) {
        _loadedMissionItems.clear();
        _loadedMissionItemsParent->deleteLater();
        _loadedMissionItemsParent = NULL;
    }

1310 1311 1312 1313 1314
    // First pass will clear old transect data, refly will append to existing data
    if (!refly) {
        _transects.clear();
        _transectsPathHeightInfo.clear();
    }
1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337

    if (_surveyAreaPolygon.count() < 3) {
        return;
    }

    // Convert polygon to NED

    QList<QPointF> polygonPoints;
    QGeoCoordinate tangentOrigin = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(0)->coordinate();
    qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" << _surveyAreaPolygon.count() << tangentOrigin;
    for (int i=0; i<_surveyAreaPolygon.count(); i++) {
        double y, x, down;
        QGeoCoordinate vertex = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(i)->coordinate();
        if (i == 0) {
            // This avoids a nan calculation that comes out of convertGeoToNed
            x = y = 0;
        } else {
            convertGeoToNed(vertex, tangentOrigin, &y, &x, &down);
        }
        polygonPoints += QPointF(x, y);
        qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
    }

1338 1339 1340 1341 1342 1343 1344
    // convert into QPolygonF
    QPolygonF polygon;
    for (int i=0; i<polygonPoints.count(); i++) {
        qCDebug(SurveyComplexItemLog) << "Vertex" << polygonPoints[i];
        polygon << polygonPoints[i];
    }

1345
    // Create list of separate polygons
1346
    QList<QPolygonF> polygons{};
1347
    qCDebug(PolygonDecomposeLog) << "*********_PolygonDecomposeConvex begin of recursion**************";
1348
    _PolygonDecomposeConvex(polygon, polygons);
1349
    qCDebug(PolygonDecomposeLog) << "polygons.size() " << polygons.size() ;
1350 1351

    // iterate over polygons
1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370
    for (auto p = polygons.begin(); p != polygons.end(); ++p) {
        QPointF* vMatch = nullptr;
        // find matching vertex in previous polygon
        if (p != polygons.begin()) {
            auto pLast = p - 1;
            for (auto& i : *p) {
                for (auto& j : *pLast) {
                   if (i == j) {
                       vMatch = &i;
                       break;
                   }
                   if (vMatch) break;
                }
            }
            if (nullptr == vMatch) qCDebug(PolygonDecomposeLog) << "no match found";

        }


1371
        // close polygon
1372
        *p << p->front();
1373 1374
        // build transects for this polygon
        // TODO figure out tangent origin
1375 1376 1377
        // TODO improve selection of entry points
//        qCDebug(SurveyComplexItemLog) << "Transects from polynom p " << p;
        _rebuildTranscetsFromPolygon(refly, *p, tangentOrigin, vMatch);
1378
    }
Thomas Gubler's avatar