QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
SurveyComplexItem.cc
Go to the documentation of this file.
1#include "SurveyComplexItem.h"
2#include "JsonParsing.h"
3#include "QGCGeo.h"
4#include "QGCQGeoCoordinate.h"
5#include "SettingsManager.h"
6#include "AppSettings.h"
8#include "MissionItem.h"
9#include "AppMessages.h"
10#include "QGCApplication.h"
11#include "Vehicle.h"
12#include "QGCLoggingCategory.h"
13
14#include <QtGui/QPolygonF>
15#include <QtCore/QJsonArray>
16#include <QtCore/QLineF>
17
18QGC_LOGGING_CATEGORY(SurveyComplexItemLog, "Plan.SurveyComplexItem")
19
20SurveyComplexItem::SurveyComplexItem(PlanMasterController* masterController, bool flyView, const QString& kmlOrShpFile)
21 : TransectStyleComplexItem (masterController, flyView, settingsGroup)
22 , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/Survey.SettingsGroup.json"), this))
23 , _gridAngleFact (settingsGroup, _metaDataMap[gridAngleName])
24 , _flyAlternateTransectsFact(settingsGroup, _metaDataMap[flyAlternateTransectsName])
25 , _splitConcavePolygonsFact (settingsGroup, _metaDataMap[splitConcavePolygonsName])
26 , _entryPoint (EntryLocationTopLeft)
27{
28 _editorQml = "qrc:/qml/QGroundControl/PlanView/SurveyItemEditor.qml";
29
30 if (_controllerVehicle && !(_controllerVehicle->fixedWing() || _controllerVehicle->vtol())) {
31 // Only fixed wing flight paths support alternate transects
32 _flyAlternateTransectsFact.setRawValue(false);
33 }
34
35 // We override the altitude to the mission default
36 if (_cameraCalc.isManualCamera() || !_cameraCalc.valueSetIsDistance()->rawValue().toBool()) {
37 _cameraCalc.distanceToSurface()->setRawValue(SettingsManager::instance()->appSettings()->defaultMissionItemAltitude()->rawValue());
38 }
39
40 connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyComplexItem::_setDirty);
41 connect(&_flyAlternateTransectsFact,&Fact::valueChanged, this, &SurveyComplexItem::_setDirty);
42 connect(&_splitConcavePolygonsFact, &Fact::valueChanged, this, &SurveyComplexItem::_setDirty);
44
45 connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyComplexItem::_rebuildTransects);
46 connect(&_flyAlternateTransectsFact,&Fact::valueChanged, this, &SurveyComplexItem::_rebuildTransects);
47 connect(&_splitConcavePolygonsFact, &Fact::valueChanged, this, &SurveyComplexItem::_rebuildTransects);
49
50 connect(&_surveyAreaPolygon, &QGCMapPolygon::isValidChanged, this, &SurveyComplexItem::_updateWizardMode);
51 connect(&_surveyAreaPolygon, &QGCMapPolygon::traceModeChanged, this, &SurveyComplexItem::_updateWizardMode);
52
53 if (!kmlOrShpFile.isEmpty()) {
54 _surveyAreaPolygon.loadKMLOrSHPFile(kmlOrShpFile);
55 _surveyAreaPolygon.setDirty(false);
56 }
57 setDirty(false);
58}
59
60void SurveyComplexItem::save(QJsonArray& planItems)
61{
62 QJsonObject saveObject;
63
64 _saveCommon(saveObject);
65 planItems.append(saveObject);
66}
67
68void SurveyComplexItem::savePreset(const QString& presetName)
69{
70 QJsonObject saveObject;
71
72 _saveCommon(saveObject);
73 _savePresetJson(presetName, saveObject);
74}
75
76void SurveyComplexItem::_saveCommon(QJsonObject& saveObject)
77{
79
80 saveObject[JsonParsing::jsonVersionKey] = 5;
83 saveObject[_jsonGridAngleKey] = _gridAngleFact.rawValue().toDouble();
84 saveObject[_jsonFlyAlternateTransectsKey] = _flyAlternateTransectsFact.rawValue().toBool();
85 saveObject[_jsonSplitConcavePolygonsKey] = _splitConcavePolygonsFact.rawValue().toBool();
86 saveObject[_jsonEntryPointKey] = _entryPoint;
87
88 // Polygon shape
90}
91
92void SurveyComplexItem::loadPreset(const QString& presetName)
93{
94 QString errorString;
95
96 QJsonObject presetObject = _loadPresetJson(presetName);
97 if (!_loadV4V5(presetObject, 0, errorString, 5, true /* forPresets */)) {
98 QGC::showAppMessage(QStringLiteral("Internal Error: Preset load failed. Name: %1 Error: %2").arg(presetName).arg(errorString));
99 }
101}
102
104{
105 Q_UNUSED(prevAltitude);
106 _cameraCalc.setDistanceMode(prevAltFrame);
107}
108
109bool SurveyComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
110{
111 // We need to pull version first to determine what validation/conversion needs to be performed
112 QList<JsonParsing::KeyValidateInfo> versionKeyInfoList = {
113 { JsonParsing::jsonVersionKey, QJsonValue::Double, true },
114 };
115 if (!JsonParsing::validateKeys(complexObject, versionKeyInfoList, errorString)) {
116 return false;
117 }
118
119 int version = complexObject[JsonParsing::jsonVersionKey].toInt();
120 if (version < 2 || version > 5) {
121 errorString = tr("Survey items do not support version %1").arg(version);
122 return false;
123 }
124
125 if (version == 4 || version == 5) {
126 if (!_loadV4V5(complexObject, sequenceNumber, errorString, version, false /* forPresets */)) {
127 return false;
128 }
129
131 if (_cameraShots == 0) {
132 // Shot count was possibly not available from plan file
133 _recalcCameraShots();
134 }
135 } else {
136 // Must be v2 or v3
137 QJsonObject v3ComplexObject = complexObject;
138 if (version == 2) {
139 // Convert to v3
140 if (v3ComplexObject.contains(VisualMissionItem::jsonTypeKey) && v3ComplexObject[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("survey")) {
143 }
144 }
145 if (!_loadV3(complexObject, sequenceNumber, errorString)) {
146 return false;
147 }
148
149 // V2/3 doesn't include individual items so we need to rebuild manually
151 }
152
153 return true;
154}
155
156bool SurveyComplexItem::_loadV4V5(const QJsonObject& complexObject, int sequenceNumber, QString& errorString, int version, bool forPresets)
157{
158 QList<JsonParsing::KeyValidateInfo> keyInfoList = {
159 { VisualMissionItem::jsonTypeKey, QJsonValue::String, true },
160 { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true },
161 { _jsonEntryPointKey, QJsonValue::Double, true },
162 { _jsonGridAngleKey, QJsonValue::Double, true },
163 { _jsonFlyAlternateTransectsKey, QJsonValue::Bool, false },
164 };
165
166 if(version == 5) {
167 JsonParsing::KeyValidateInfo jSplitPolygon = { _jsonSplitConcavePolygonsKey, QJsonValue::Bool, true };
168 keyInfoList.append(jSplitPolygon);
169 }
170
171 if (!JsonParsing::validateKeys(complexObject, keyInfoList, errorString)) {
172 return false;
173 }
174
175 QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
176 QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
178 errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
179 return false;
180 }
181
182 _ignoreRecalc = !forPresets;
183
184 if (!forPresets) {
186
187 if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) {
189 return false;
190 }
191 }
192
193 if (!TransectStyleComplexItem::_load(complexObject, forPresets, errorString)) {
194 _ignoreRecalc = false;
195 return false;
196 }
197
198 _gridAngleFact.setRawValue (complexObject[_jsonGridAngleKey].toDouble());
199 _flyAlternateTransectsFact.setRawValue (complexObject[_jsonFlyAlternateTransectsKey].toBool(false));
200
201 if (version == 5) {
202 _splitConcavePolygonsFact.setRawValue (complexObject[_jsonSplitConcavePolygonsKey].toBool(true));
203 }
204
205 _entryPoint = complexObject[_jsonEntryPointKey].toInt();
206
207 _ignoreRecalc = false;
208
209 return true;
210}
211
212bool SurveyComplexItem::_loadV3(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
213{
214 QList<JsonParsing::KeyValidateInfo> mainKeyInfoList = {
215 { VisualMissionItem::jsonTypeKey, QJsonValue::String, true },
216 { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true },
217 { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true },
218 { _jsonV3GridObjectKey, QJsonValue::Object, true },
219 { _jsonV3CameraObjectKey, QJsonValue::Object, false },
220 { _jsonV3CameraTriggerDistanceKey, QJsonValue::Double, true },
221 { _jsonV3ManualGridKey, QJsonValue::Bool, true },
222 { _jsonV3FixedValueIsAltitudeKey, QJsonValue::Bool, true },
223 { _jsonV3HoverAndCaptureKey, QJsonValue::Bool, false },
224 { _jsonV3Refly90DegreesKey, QJsonValue::Bool, false },
225 { _jsonV3CameraTriggerInTurnaroundKey, QJsonValue::Bool, false }, // Should really be required, but it was missing from initial code due to bug
226 };
227 if (!JsonParsing::validateKeys(complexObject, mainKeyInfoList, errorString)) {
228 return false;
229 }
230
231 QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
232 QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
234 errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
235 return false;
236 }
237
238 _ignoreRecalc = true;
239
241
242 _hoverAndCaptureFact.setRawValue (complexObject[_jsonV3HoverAndCaptureKey].toBool(false));
243 _refly90DegreesFact.setRawValue (complexObject[_jsonV3Refly90DegreesKey].toBool(false));
244 _cameraTriggerInTurnAroundFact.setRawValue (complexObject[_jsonV3CameraTriggerInTurnaroundKey].toBool(true));
245
246 _cameraCalc.valueSetIsDistance()->setRawValue (complexObject[_jsonV3FixedValueIsAltitudeKey].toBool(true));
247 _cameraCalc.setDistanceMode(complexObject[_jsonV3GridAltitudeRelativeKey].toBool(true) ? QGroundControlQmlGlobal::AltitudeFrameRelative : QGroundControlQmlGlobal::AltitudeFrameAbsolute);
248
249 bool manualGrid = complexObject[_jsonV3ManualGridKey].toBool(true);
250
251 QList<JsonParsing::KeyValidateInfo> gridKeyInfoList = {
252 { _jsonV3GridAltitudeKey, QJsonValue::Double, true },
253 { _jsonV3GridAltitudeRelativeKey, QJsonValue::Bool, true },
254 { _jsonV3GridAngleKey, QJsonValue::Double, true },
255 { _jsonV3GridSpacingKey, QJsonValue::Double, true },
256 { _jsonEntryPointKey, QJsonValue::Double, false },
257 { _jsonV3TurnaroundDistKey, QJsonValue::Double, true },
258 };
259 QJsonObject gridObject = complexObject[_jsonV3GridObjectKey].toObject();
260 if (!JsonParsing::validateKeys(gridObject, gridKeyInfoList, errorString)) {
261 _ignoreRecalc = false;
262 return false;
263 }
264
265 _gridAngleFact.setRawValue (gridObject[_jsonV3GridAngleKey].toDouble());
266 _turnAroundDistanceFact.setRawValue (gridObject[_jsonV3TurnaroundDistKey].toDouble());
267
268 if (gridObject.contains(_jsonEntryPointKey)) {
269 _entryPoint = gridObject[_jsonEntryPointKey].toInt();
270 } else {
271 _entryPoint = EntryLocationTopRight;
272 }
273
274 _cameraCalc.distanceToSurface()->setRawValue (gridObject[_jsonV3GridAltitudeKey].toDouble());
275 _cameraCalc.adjustedFootprintSide()->setRawValue (gridObject[_jsonV3GridSpacingKey].toDouble());
276 _cameraCalc.adjustedFootprintFrontal()->setRawValue (complexObject[_jsonV3CameraTriggerDistanceKey].toDouble());
277
278 if (manualGrid) {
280 } else {
281 if (!complexObject.contains(_jsonV3CameraObjectKey)) {
282 errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera");
283 _ignoreRecalc = false;
284 return false;
285 }
286
287 QJsonObject cameraObject = complexObject[_jsonV3CameraObjectKey].toObject();
288
289 // Older code had typo on "imageSideOverlap" incorrectly being "imageSizeOverlap"
290 QString incorrectImageSideOverlap = "imageSizeOverlap";
291 if (cameraObject.contains(incorrectImageSideOverlap)) {
292 cameraObject[_jsonV3SideOverlapKey] = cameraObject[incorrectImageSideOverlap];
293 cameraObject.remove(incorrectImageSideOverlap);
294 }
295
296 QList<JsonParsing::KeyValidateInfo> cameraKeyInfoList = {
297 { _jsonV3GroundResolutionKey, QJsonValue::Double, true },
298 { _jsonV3FrontalOverlapKey, QJsonValue::Double, true },
299 { _jsonV3SideOverlapKey, QJsonValue::Double, true },
300 { _jsonV3CameraSensorWidthKey, QJsonValue::Double, true },
301 { _jsonV3CameraSensorHeightKey, QJsonValue::Double, true },
302 { _jsonV3CameraResolutionWidthKey, QJsonValue::Double, true },
303 { _jsonV3CameraResolutionHeightKey, QJsonValue::Double, true },
304 { _jsonV3CameraFocalLengthKey, QJsonValue::Double, true },
305 { _jsonV3CameraNameKey, QJsonValue::String, true },
306 { _jsonV3CameraOrientationLandscapeKey, QJsonValue::Bool, true },
307 { _jsonV3CameraMinTriggerIntervalKey, QJsonValue::Double, false },
308 };
309 if (!JsonParsing::validateKeys(cameraObject, cameraKeyInfoList, errorString)) {
310 _ignoreRecalc = false;
311 return false;
312 }
313
314 _cameraCalc.landscape()->setRawValue (cameraObject[_jsonV3CameraOrientationLandscapeKey].toBool(true));
315 _cameraCalc.frontalOverlap()->setRawValue (cameraObject[_jsonV3FrontalOverlapKey].toInt());
316 _cameraCalc.sideOverlap()->setRawValue (cameraObject[_jsonV3SideOverlapKey].toInt());
317 _cameraCalc.sensorWidth()->setRawValue (cameraObject[_jsonV3CameraSensorWidthKey].toDouble());
318 _cameraCalc.sensorHeight()->setRawValue (cameraObject[_jsonV3CameraSensorHeightKey].toDouble());
319 _cameraCalc.focalLength()->setRawValue (cameraObject[_jsonV3CameraFocalLengthKey].toDouble());
320 _cameraCalc.imageWidth()->setRawValue (cameraObject[_jsonV3CameraResolutionWidthKey].toInt());
321 _cameraCalc.imageHeight()->setRawValue (cameraObject[_jsonV3CameraResolutionHeightKey].toInt());
322 _cameraCalc.minTriggerInterval()->setRawValue (cameraObject[_jsonV3CameraMinTriggerIntervalKey].toDouble(0));
323 _cameraCalc.imageDensity()->setRawValue (cameraObject[_jsonV3GroundResolutionKey].toDouble());
325 _cameraCalc._setCameraNameFromV3TransectLoad (cameraObject[_jsonV3CameraNameKey].toString());
326 }
327
328 // Polygon shape
334 if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) {
336 _ignoreRecalc = false;
337 return false;
338 }
339
340 _ignoreRecalc = false;
341
342 return true;
343}
344
346void SurveyComplexItem::_reverseTransectOrder(QList<QList<QGeoCoordinate>>& transects)
347{
348 QList<QList<QGeoCoordinate>> rgReversedTransects;
349 for (int i=transects.count() - 1; i>=0; i--) {
350 rgReversedTransects.append(transects[i]);
351 }
352 transects = rgReversedTransects;
353}
354
356void SurveyComplexItem::_reverseInternalTransectPoints(QList<QList<QGeoCoordinate>>& transects)
357{
358 for (int i=0; i<transects.count(); i++) {
359 QList<QGeoCoordinate> rgReversedCoords;
360 QList<QGeoCoordinate>& rgOriginalCoords = transects[i];
361 for (int j=rgOriginalCoords.count()-1; j>=0; j--) {
362 rgReversedCoords.append(rgOriginalCoords[j]);
363 }
364 transects[i] = rgReversedCoords;
365 }
366}
367
372void SurveyComplexItem::_optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList<QList<QGeoCoordinate>>& transects)
373{
374 double rgTransectDistance[4];
375 rgTransectDistance[0] = transects.first().first().distanceTo(distanceCoord);
376 rgTransectDistance[1] = transects.first().last().distanceTo(distanceCoord);
377 rgTransectDistance[2] = transects.last().first().distanceTo(distanceCoord);
378 rgTransectDistance[3] = transects.last().last().distanceTo(distanceCoord);
379
380 int shortestIndex = 0;
381 double shortestDistance = rgTransectDistance[0];
382 for (int i=1; i<3; i++) {
383 if (rgTransectDistance[i] < shortestDistance) {
384 shortestIndex = i;
385 shortestDistance = rgTransectDistance[i];
386 }
387 }
388
389 if (shortestIndex > 1) {
390 // We need to reverse the order of segments
391 _reverseTransectOrder(transects);
392 }
393 if (shortestIndex & 1) {
394 // We need to reverse the points within each segment
395 _reverseInternalTransectPoints(transects);
396 }
397}
398
399qreal SurveyComplexItem::_ccw(QPointF pt1, QPointF pt2, QPointF pt3)
400{
401 return (pt2.x()-pt1.x())*(pt3.y()-pt1.y()) - (pt2.y()-pt1.y())*(pt3.x()-pt1.x());
402}
403
404qreal SurveyComplexItem::_dp(QPointF pt1, QPointF pt2)
405{
406 return (pt2.x()-pt1.x())/qSqrt((pt2.x()-pt1.x())*(pt2.x()-pt1.x()) + (pt2.y()-pt1.y())*(pt2.y()-pt1.y()));
407}
408
409void SurveyComplexItem::_swapPoints(QList<QPointF>& points, int index1, int index2)
410{
411 QPointF temp = points[index1];
412 points[index1] = points[index2];
413 points[index2] = temp;
414}
415
417bool SurveyComplexItem::_gridAngleIsNorthSouthTransects()
418{
419 // Grid angle ranges from -360<->360
420 double gridAngle = qAbs(_gridAngleFact.rawValue().toDouble());
421 return gridAngle < 45.0 || (gridAngle > 360.0 - 45.0) || (gridAngle > 90.0 + 45.0 && gridAngle < 270.0 - 45.0);
422}
423
424void SurveyComplexItem::_adjustTransectsToEntryPointLocation(QList<QList<QGeoCoordinate>>& transects)
425{
426 if (transects.count() == 0) {
427 return;
428 }
429
430 bool reversePoints = false;
431 bool reverseTransects = false;
432
433 if (_entryPoint == EntryLocationBottomLeft || _entryPoint == EntryLocationBottomRight) {
434 reversePoints = true;
435 }
436 if (_entryPoint == EntryLocationTopRight || _entryPoint == EntryLocationBottomRight) {
437 reverseTransects = true;
438 }
439
440 if (reversePoints) {
441 qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Reverse Points";
442 _reverseInternalTransectPoints(transects);
443 }
444 if (reverseTransects) {
445 qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Reverse Transects";
446 _reverseTransectOrder(transects);
447 }
448
449 qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Modified entry point:entryLocation" << transects.first().first() << _entryPoint;
450}
451
452QPointF SurveyComplexItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
453{
454 QPointF rotated;
455 double radians = (M_PI / 180.0) * -angle;
456
457 rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x());
458 rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y());
459
460 return rotated;
461}
462
463void SurveyComplexItem::_intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines)
464{
465 QLineF topLine (boundRect.topLeft(), boundRect.topRight());
466 QLineF bottomLine (boundRect.bottomLeft(), boundRect.bottomRight());
467 QLineF leftLine (boundRect.topLeft(), boundRect.bottomLeft());
468 QLineF rightLine (boundRect.topRight(), boundRect.bottomRight());
469
470 for (int i=0; i<lineList.count(); i++) {
471 QPointF intersectPoint;
472 QLineF intersectLine;
473 const QLineF& line = lineList[i];
474
475 auto isLineBoundedIntersect = [&line, &intersectPoint](const QLineF& linePosition) {
476 return line.intersects(linePosition, &intersectPoint) == QLineF::BoundedIntersection;
477 };
478
479 int foundCount = 0;
480 if (isLineBoundedIntersect(topLine)) {
481 intersectLine.setP1(intersectPoint);
482 foundCount++;
483 }
484 if (isLineBoundedIntersect(rightLine)) {
485 if (foundCount == 0) {
486 intersectLine.setP1(intersectPoint);
487 } else {
488 if (foundCount != 1) {
489 qWarning() << "Found more than two intersecting points";
490 }
491 intersectLine.setP2(intersectPoint);
492 }
493 foundCount++;
494 }
495 if (isLineBoundedIntersect(bottomLine)) {
496 if (foundCount == 0) {
497 intersectLine.setP1(intersectPoint);
498 } else {
499 if (foundCount != 1) {
500 qWarning() << "Found more than two intersecting points";
501 }
502 intersectLine.setP2(intersectPoint);
503 }
504 foundCount++;
505 }
506 if (isLineBoundedIntersect(leftLine)) {
507 if (foundCount == 0) {
508 intersectLine.setP1(intersectPoint);
509 } else {
510 if (foundCount != 1) {
511 qWarning() << "Found more than two intersecting points";
512 }
513 intersectLine.setP2(intersectPoint);
514 }
515 foundCount++;
516 }
517
518 if (foundCount == 2) {
519 resultLines += intersectLine;
520 }
521 }
522}
523
524void SurveyComplexItem::_intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines)
525{
526 resultLines.clear();
527
528 for (int i=0; i<lineList.count(); i++) {
529 const QLineF& line = lineList[i];
530 QList<QPointF> intersections;
531
532 // Intersect the line with all the polygon edges
533 for (int j=0; j<polygon.count()-1; j++) {
534 QPointF intersectPoint;
535 QLineF polygonLine = QLineF(polygon[j], polygon[j+1]);
536
537 auto intersect = line.intersects(polygonLine, &intersectPoint);
538 if (intersect == QLineF::BoundedIntersection) {
539 if (!intersections.contains(intersectPoint)) {
540 intersections.append(intersectPoint);
541 }
542 }
543 }
544
545 // We now have one or more intersection points all along the same line. Find the two
546 // which are furthest away from each other to form the transect.
547 if (intersections.count() > 1) {
548 QPointF firstPoint;
549 QPointF secondPoint;
550 double currentMaxDistance = 0;
551
552 for (int intersectionIndex=0; intersectionIndex<intersections.count(); intersectionIndex++) {
553 for (int compareIndex=0; compareIndex<intersections.count(); compareIndex++) {
554 QLineF lineTest(intersections[intersectionIndex], intersections[compareIndex]);
555 \
556 double newMaxDistance = lineTest.length();
557 if (newMaxDistance > currentMaxDistance) {
558 firstPoint = intersections[intersectionIndex];
559 secondPoint = intersections[compareIndex];
560 currentMaxDistance = newMaxDistance;
561 }
562 }
563 }
564
565 resultLines += QLineF(firstPoint, secondPoint);
566 }
567 }
568}
569
571void SurveyComplexItem::_adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines)
572{
573 qreal firstAngle = 0;
574 for (int i=0; i<lineList.count(); i++) {
575 const QLineF& line = lineList[i];
576 QLineF adjustedLine;
577
578 if (i == 0) {
579 firstAngle = line.angle();
580 }
581
582 if (qAbs(line.angle() - firstAngle) > 1.0) {
583 adjustedLine.setP1(line.p2());
584 adjustedLine.setP2(line.p1());
585 } else {
586 adjustedLine = line;
587 }
588
589 resultLines += adjustedLine;
590 }
591}
592
593double SurveyComplexItem::_clampGridAngle90(double gridAngle)
594{
595 // Clamp grid angle to -90<->90. This prevents transects from being rotated to a reversed order.
596 if (gridAngle > 90.0) {
597 gridAngle -= 180.0;
598 } else if (gridAngle < -90.0) {
599 gridAngle += 180;
600 }
601 return gridAngle;
602}
603
604bool SurveyComplexItem::_nextTransectCoord(const QList<QGeoCoordinate>& transectPoints, int pointIndex, QGeoCoordinate& coord)
605{
606 if (pointIndex > transectPoints.count()) {
607 qWarning() << "Bad grid generation";
608 return false;
609 }
610
611 coord = transectPoints[pointIndex];
612 return true;
613}
614
615bool SurveyComplexItem::_hasTurnaround(void) const
616{
617 return _turnAroundDistance() > 0;
618}
619
620double SurveyComplexItem::_turnaroundDistance(void) const
621{
622 return _turnAroundDistanceFact.rawValue().toDouble();
623}
624
625void SurveyComplexItem::_rebuildTransectsPhase1(void)
626{
627 _rebuildTransectsPhase1WorkerSinglePolygon(false /* refly */);
628 if (_refly90DegreesFact.rawValue().toBool()) {
629 _rebuildTransectsPhase1WorkerSinglePolygon(true /* refly */);
630 }
631}
632
633void SurveyComplexItem::_rebuildTransectsPhase1WorkerSinglePolygon(bool refly)
634{
635 if (_ignoreRecalc) {
636 return;
637 }
638
639 // If the transects are getting rebuilt then any previously loaded mission items are now invalid
641 _loadedMissionItems.clear();
642 _loadedMissionItemsParent->deleteLater();
644 }
645
646 if (_surveyAreaPolygon.count() < 3) {
647 return;
648 }
649
650 // Convert polygon to NED
651
652 QList<QPointF> polygonPoints;
653 QGeoCoordinate tangentOrigin = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(0)->coordinate();
654 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" << _surveyAreaPolygon.count() << tangentOrigin;
655 for (int i=0; i<_surveyAreaPolygon.count(); i++) {
656 double y, x, down;
657 QGeoCoordinate vertex = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(i)->coordinate();
658 if (i == 0) {
659 // This avoids a nan calculation that comes out of convertGeoToNed
660 x = y = 0;
661 } else {
662 QGCGeo::convertGeoToNed(vertex, tangentOrigin, y, x, down);
663 }
664 polygonPoints += QPointF(x, y);
665 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
666 }
667
668 // Generate transects
669
670 double gridAngle = _gridAngleFact.rawValue().toDouble();
671 double gridSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
672 if (gridSpacing < _minimumTransectSpacingMeters) {
673 // We can't let spacing get too small otherwise we will end up with too many transects.
674 // So we limit the spacing to be above a small increment and below that value we set to huge spacing
675 // which will cause a single transect to be added instead of having things blow up.
677 }
678
679 gridAngle = _clampGridAngle90(gridAngle);
680 gridAngle += refly ? 90 : 0;
681 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Clamped grid angle" << gridAngle;
682
683 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly;
684
685 // Convert polygon to bounding rect
686
687 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Polygon";
688 QPolygonF polygon;
689 for (int i=0; i<polygonPoints.count(); i++) {
690 qCDebug(SurveyComplexItemLog) << "Vertex" << polygonPoints[i];
691 polygon << polygonPoints[i];
692 }
693 polygon << polygonPoints[0];
694 QRectF boundingRect = polygon.boundingRect();
695 QPointF boundingCenter = boundingRect.center();
696 qCDebug(SurveyComplexItemLog) << "Bounding rect" << boundingRect.topLeft().x() << boundingRect.topLeft().y() << boundingRect.bottomRight().x() << boundingRect.bottomRight().y();
697
698 // Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
699 // bounding box to guarantee intersection.
700
701 QList<QLineF> lineList;
702
703 // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor.
704 // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to.
705 // They are initially generated with the transects flowing from west to east and then points within the transect north to south.
706 double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0;
707 double halfWidth = maxWidth / 2.0;
708 double transectX = boundingCenter.x() - halfWidth;
709 double transectXMax = transectX + maxWidth;
710 while (transectX < transectXMax) {
711 double transectYTop = boundingCenter.y() - halfWidth;
712 double transectYBottom = boundingCenter.y() + halfWidth;
713
714 lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle));
715 transectX += gridSpacing;
716 }
717
718 // Now intersect the lines with the polygon
719 QList<QLineF> intersectLines;
720#if 1
721 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
722#else
723 // This is handy for debugging grid problems, not for release
724 intersectLines = lineList;
725#endif
726
727 // Less than two transects intersected with the polygon:
728 // Create a single transect which goes through the center of the polygon
729 // Intersect it with the polygon
730 if (intersectLines.count() < 2) {
732 QLineF firstLine = lineList.first();
733 QPointF lineCenter = firstLine.pointAt(0.5);
734 QPointF centerOffset = boundingCenter - lineCenter;
735 firstLine.translate(centerOffset);
736 lineList.clear();
737 lineList.append(firstLine);
738 intersectLines = lineList;
739 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
740 }
741
742 // Make sure all lines are going the same direction. Polygon intersection leads to lines which
743 // can be in varied directions depending on the order of the intesecting sides.
744 QList<QLineF> resultLines;
745 _adjustLineDirection(intersectLines, resultLines);
746
747 // Convert from NED to Geo
748 QList<QList<QGeoCoordinate>> transects;
749 for (const QLineF& line : resultLines) {
750 QGeoCoordinate coord;
751 QList<QGeoCoordinate> transect;
752
753 QGCGeo::convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, coord);
754 transect.append(coord);
755 QGCGeo::convertNedToGeo(line.p2().y(), line.p2().x(), 0, tangentOrigin, coord);
756 transect.append(coord);
757
758 transects.append(transect);
759 }
760
761 _adjustTransectsToEntryPointLocation(transects);
762
763 if (refly) {
764 _optimizeTransectsForShortestDistance(_transects.last().last().coord, transects);
765 }
766
767 if (_flyAlternateTransectsFact.rawValue().toBool()) {
768 QList<QList<QGeoCoordinate>> alternatingTransects;
769 for (int i=0; i<transects.count(); i++) {
770 if (!(i & 1)) {
771 alternatingTransects.append(transects[i]);
772 }
773 }
774 for (int i=transects.count()-1; i>0; i--) {
775 if (i & 1) {
776 alternatingTransects.append(transects[i]);
777 }
778 }
779 transects = alternatingTransects;
780 }
781
782 // Adjust to lawnmower pattern
783 bool reverseVertices = false;
784 for (int i=0; i<transects.count(); i++) {
785 // We must reverse the vertices for every other transect in order to make a lawnmower pattern
786 QList<QGeoCoordinate> transectVertices = transects[i];
787 if (reverseVertices) {
788 reverseVertices = false;
789 QList<QGeoCoordinate> reversedVertices;
790 for (int j=transectVertices.count()-1; j>=0; j--) {
791 reversedVertices.append(transectVertices[j]);
792 }
793 transectVertices = reversedVertices;
794 } else {
795 reverseVertices = true;
796 }
797 transects[i] = transectVertices;
798 }
799
800 // Convert to CoordInfo transects and append to _transects
801 for (const QList<QGeoCoordinate>& transect : transects) {
802 QGeoCoordinate coord;
803 QList<TransectStyleComplexItem::CoordInfo_t> coordInfoTransect;
805
806 coordInfo = { transect[0], CoordTypeSurveyEntry };
807 coordInfoTransect.append(coordInfo);
808 coordInfo = { transect[1], CoordTypeSurveyExit };
809 coordInfoTransect.append(coordInfo);
810
811 // For hover and capture we need points for each camera location within the transect
813 double transectLength = transect[0].distanceTo(transect[1]);
814 double transectAzimuth = transect[0].azimuthTo(transect[1]);
815 if (triggerDistance() < transectLength) {
816 int cInnerHoverPoints = static_cast<int>(floor(transectLength / triggerDistance()));
817 qCDebug(SurveyComplexItemLog) << "cInnerHoverPoints" << cInnerHoverPoints;
818 for (int i=0; i<cInnerHoverPoints; i++) {
819 QGeoCoordinate hoverCoord = transect[0].atDistanceAndAzimuth(triggerDistance() * (i + 1), transectAzimuth);
821 coordInfoTransect.insert(1 + i, hoverCoordInfo);
822 }
823 }
824 }
825
826 // Extend the transect ends for turnaround
827 if (_hasTurnaround()) {
828 QGeoCoordinate turnaroundCoord;
830
831 double azimuth = transect[0].azimuthTo(transect[1]);
832 turnaroundCoord = transect[0].atDistanceAndAzimuth(-turnAroundDistance, azimuth);
833 turnaroundCoord.setAltitude(qQNaN());
834 TransectStyleComplexItem::CoordInfo_t turnaroundCoordInfo = { turnaroundCoord, CoordTypeTurnaround };
835 coordInfoTransect.prepend(turnaroundCoordInfo);
836
837 azimuth = transect.last().azimuthTo(transect[transect.count() - 2]);
838 turnaroundCoord = transect.last().atDistanceAndAzimuth(-turnAroundDistance, azimuth);
839 turnaroundCoord.setAltitude(qQNaN());
840 coordInfo = { turnaroundCoord, CoordTypeTurnaround };
841 coordInfoTransect.append(coordInfo);
842 }
843
844 _transects.append(coordInfoTransect);
845 }
846}
847
848#if 0
849 // Splitting polygons is not supported since this code would get stuck in a infinite loop
850 // Code is left here in case someone wants to try to resurrect it
851
852void SurveyComplexItem::_rebuildTransectsPhase1WorkerSplitPolygons(bool refly)
853{
854 if (_ignoreRecalc) {
855 return;
856 }
857
858 // If the transects are getting rebuilt then any previously loaded mission items are now invalid
860 _loadedMissionItems.clear();
861 _loadedMissionItemsParent->deleteLater();
863 }
864
865 if (_surveyAreaPolygon.count() < 3) {
866 return;
867 }
868
869 // Convert polygon to NED
870
871 QList<QPointF> polygonPoints;
872 QGeoCoordinate tangentOrigin = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(0)->coordinate();
873 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" << _surveyAreaPolygon.count() << tangentOrigin;
874 for (int i=0; i<_surveyAreaPolygon.count(); i++) {
875 double y, x, down;
876 QGeoCoordinate vertex = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(i)->coordinate();
877 if (i == 0) {
878 // This avoids a nan calculation that comes out of convertGeoToNed
879 x = y = 0;
880 } else {
881 convertGeoToNed(vertex, tangentOrigin, y, x, down);
882 }
883 polygonPoints += QPointF(x, y);
884 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
885 }
886
887 // convert into QPolygonF
888 QPolygonF polygon;
889 for (int i=0; i<polygonPoints.count(); i++) {
890 qCDebug(SurveyComplexItemLog) << "Vertex" << polygonPoints[i];
891 polygon << polygonPoints[i];
892 }
893
894 // Create list of separate polygons
895 QList<QPolygonF> polygons{};
896 _PolygonDecomposeConvex(polygon, polygons);
897
898 // iterate over polygons
899 for (auto p = polygons.begin(); p != polygons.end(); ++p) {
900 QPointF* vMatch = nullptr;
901 // find matching vertex in previous polygon
902 if (p != polygons.begin()) {
903 auto pLast = p - 1;
904 for (auto& i : *p) {
905 for (auto& j : *pLast) {
906 if (i == j) {
907 vMatch = &i;
908 break;
909 }
910 if (vMatch) break;
911 }
912 }
913
914 }
915
916
917 // close polygon
918 *p << p->front();
919 // build transects for this polygon
920 // TODO figure out tangent origin
921 // TODO improve selection of entry points
922// qCDebug(SurveyComplexItemLog) << "Transects from polynom p " << p;
923 _rebuildTransectsFromPolygon(refly, *p, tangentOrigin, vMatch);
924 }
925}
926
927void SurveyComplexItem::_PolygonDecomposeConvex(const QPolygonF& polygon, QList<QPolygonF>& decomposedPolygons)
928{
929 // this follows "Mark Keil's Algorithm" https://mpen.ca/406/keil
930 int decompSize = std::numeric_limits<int>::max();
931 if (polygon.size() < 3) return;
932 if (polygon.size() == 3) {
933 decomposedPolygons << polygon;
934 return;
935 }
936
937 QList<QPolygonF> decomposedPolygonsMin{};
938
939 for (auto vertex = polygon.begin(); vertex != polygon.end(); ++vertex)
940 {
941 // is vertex reflex?
942 bool vertexIsReflex = _VertexIsReflex(polygon, vertex);
943
944 if (!vertexIsReflex) continue;
945
946 for (auto vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther)
947 {
948 auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
949 auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
950 if (vertexOther == vertex) continue;
951 if (vertexAfter == vertexOther) continue;
952 if (vertexBefore == vertexOther) continue;
953 bool canSee = _VertexCanSeeOther(polygon, vertex, vertexOther);
954 if (!canSee) continue;
955
956 QPolygonF polyLeft;
957 auto v = vertex;
958 auto polyLeftContainsReflex = false;
959 while ( v != vertexOther) {
960 if (v != vertex && _VertexIsReflex(polygon, v)) {
961 polyLeftContainsReflex = true;
962 }
963 polyLeft << *v;
964 ++v;
965 if (v == polygon.end()) v = polygon.begin();
966 }
967 polyLeft << *vertexOther;
968 auto polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3);
969
970 QPolygonF polyRight;
971 v = vertexOther;
972 auto polyRightContainsReflex = false;
973 while ( v != vertex) {
974 if (v != vertex && _VertexIsReflex(polygon, v)) {
975 polyRightContainsReflex = true;
976 }
977 polyRight << *v;
978 ++v;
979 if (v == polygon.end()) v = polygon.begin();
980 }
981 polyRight << *vertex;
982 auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3);
983
984 if (!polyLeftValid || ! polyRightValid) {
985// decompSize = std::numeric_limits<int>::max();
986 continue;
987 }
988
989 // recursion
990 QList<QPolygonF> polyLeftDecomposed{};
991 _PolygonDecomposeConvex(polyLeft, polyLeftDecomposed);
992
993 QList<QPolygonF> polyRightDecomposed{};
994 _PolygonDecomposeConvex(polyRight, polyRightDecomposed);
995
996 // compositon
997 auto subSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
998 if ((polyLeftContainsReflex && polyLeftDecomposed.size() == 1)
999 || (polyRightContainsReflex && polyRightDecomposed.size() == 1))
1000 {
1001 // don't accept polygons that contian reflex vertices and were not split
1002 subSize = std::numeric_limits<int>::max();
1003 }
1004 if (subSize < decompSize) {
1005 decompSize = subSize;
1006 decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
1007 }
1008 }
1009
1010 }
1011
1012 // assemble output
1013 if (decomposedPolygonsMin.size() > 0) {
1014 decomposedPolygons << decomposedPolygonsMin;
1015 } else {
1016 decomposedPolygons << polygon;
1017 }
1018
1019 return;
1020}
1021
1022bool SurveyComplexItem::_VertexCanSeeOther(const QPolygonF& polygon, const QPointF* vertexA, const QPointF* vertexB) {
1023 if (vertexA == vertexB) return false;
1024 auto vertexAAfter = vertexA + 1 == polygon.end() ? polygon.begin() : vertexA + 1;
1025 auto vertexABefore = vertexA == polygon.begin() ? polygon.end() - 1 : vertexA - 1;
1026 if (vertexAAfter == vertexB) return false;
1027 if (vertexABefore == vertexB) return false;
1028// qCDebug(SurveyComplexItemLog) << "_VertexCanSeeOther false after first checks ";
1029
1030 bool visible = true;
1031// auto diff = *vertexA - *vertexB;
1032 QLineF lineAB{*vertexA, *vertexB};
1033 auto distanceAB = lineAB.length();//sqrtf(diff.x() * diff.x() + diff.y()*diff.y());
1034
1035// qCDebug(SurveyComplexItemLog) << "_VertexCanSeeOther distanceAB " << distanceAB;
1036 for (auto vertexC = polygon.begin(); vertexC != polygon.end(); ++vertexC)
1037 {
1038 if (vertexC == vertexA) continue;
1039 if (vertexC == vertexB) continue;
1040 auto vertexD = vertexC + 1 == polygon.end() ? polygon.begin() : vertexC + 1;
1041 if (vertexD == vertexA) continue;
1042 if (vertexD == vertexB) continue;
1043 QLineF lineCD(*vertexC, *vertexD);
1044 QPointF intersection{};
1045
1046 auto intersects = lineAB.intersects(lineCD, &intersection);
1047 if (intersects == QLineF::IntersectType::BoundedIntersection) {
1048// auto diffIntersection = *vertexA - intersection;
1049// auto distanceIntersection = sqrtf(diffIntersection.x() * diffIntersection.x() + diffIntersection.y()*diffIntersection.y());
1050// qCDebug(SurveyComplexItemLog) << "*vertexA " << *vertexA << "*vertexB " << *vertexB << " intersection " << intersection;
1051
1052 QLineF lineIntersection{*vertexA, intersection};
1053 auto distanceIntersection = lineIntersection.length();//sqrtf(diff.x() * diff.x() + diff.y()*diff.y());
1054 qCDebug(SurveyComplexItemLog) << "_VertexCanSeeOther distanceIntersection " << distanceIntersection;
1055 if (distanceIntersection < distanceAB) {
1056 visible = false;
1057 break;
1058 }
1059 }
1060
1061 }
1062
1063 return visible;
1064}
1065
1066bool SurveyComplexItem::_VertexIsReflex(const QPolygonF& polygon, QList<QPointF>::const_iterator& vertexIter) {
1067 auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
1068 auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
1069 auto area = (((vertex->x() - vertexBefore->x())*(vertexAfter->y() - vertexBefore->y()))-((vertexAfter->x() - vertexBefore->x())*(vertex->y() - vertexBefore->y())));
1070 return area > 0;
1071
1072}
1073#endif
1074
1075void SurveyComplexItem::_rebuildTransectsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin, const QPointF* const transitionPoint)
1076{
1077 // Generate transects
1078
1079 double gridAngle = _gridAngleFact.rawValue().toDouble();
1080 double gridSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
1081
1082 gridAngle = _clampGridAngle90(gridAngle);
1083 gridAngle += refly ? 90 : 0;
1084 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Clamped grid angle" << gridAngle;
1085
1086 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly;
1087
1088 // Convert polygon to bounding rect
1089
1090 qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Polygon";
1091 QRectF boundingRect = polygon.boundingRect();
1092 QPointF boundingCenter = boundingRect.center();
1093 qCDebug(SurveyComplexItemLog) << "Bounding rect" << boundingRect.topLeft().x() << boundingRect.topLeft().y() << boundingRect.bottomRight().x() << boundingRect.bottomRight().y();
1094
1095 // Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
1096 // bounding box to guarantee intersection.
1097
1098 QList<QLineF> lineList;
1099
1100 // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor.
1101 // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to.
1102 // They are initially generated with the transects flowing from west to east and then points within the transect north to south.
1103 double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0;
1104 double halfWidth = maxWidth / 2.0;
1105 double transectX = boundingCenter.x() - halfWidth;
1106 double transectXMax = transectX + maxWidth;
1107 while (transectX < transectXMax) {
1108 double transectYTop = boundingCenter.y() - halfWidth;
1109 double transectYBottom = boundingCenter.y() + halfWidth;
1110
1111 lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle));
1112 transectX += gridSpacing;
1113 }
1114
1115 // Now intersect the lines with the polygon
1116 QList<QLineF> intersectLines;
1117#if 1
1118 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
1119#else
1120 // This is handy for debugging grid problems, not for release
1121 intersectLines = lineList;
1122#endif
1123
1124 // Less than two transects intersected with the polygon:
1125 // Create a single transect which goes through the center of the polygon
1126 // Intersect it with the polygon
1127 if (intersectLines.count() < 2) {
1129 QLineF firstLine = lineList.first();
1130 QPointF lineCenter = firstLine.pointAt(0.5);
1131 QPointF centerOffset = boundingCenter - lineCenter;
1132 firstLine.translate(centerOffset);
1133 lineList.clear();
1134 lineList.append(firstLine);
1135 intersectLines = lineList;
1136 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
1137 }
1138
1139 // Make sure all lines are going the same direction. Polygon intersection leads to lines which
1140 // can be in varied directions depending on the order of the intesecting sides.
1141 QList<QLineF> resultLines;
1142 _adjustLineDirection(intersectLines, resultLines);
1143
1144 // Convert from NED to Geo
1145 QList<QList<QGeoCoordinate>> transects;
1146
1147 if (transitionPoint != nullptr) {
1148 QList<QGeoCoordinate> transect;
1149 QGeoCoordinate coord;
1150 QGCGeo::convertNedToGeo(transitionPoint->y(), transitionPoint->x(), 0, tangentOrigin, coord);
1151 transect.append(coord);
1152 transect.append(coord); //TODO
1153 transects.append(transect);
1154 }
1155
1156 for (const QLineF& line: resultLines) {
1157 QList<QGeoCoordinate> transect;
1158 QGeoCoordinate coord;
1159
1160 QGCGeo::convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, coord);
1161 transect.append(coord);
1162 QGCGeo::convertNedToGeo(line.p2().y(), line.p2().x(), 0, tangentOrigin, coord);
1163 transect.append(coord);
1164
1165 transects.append(transect);
1166 }
1167
1168 _adjustTransectsToEntryPointLocation(transects);
1169
1170 if (refly) {
1171 _optimizeTransectsForShortestDistance(_transects.last().last().coord, transects);
1172 }
1173
1174 if (_flyAlternateTransectsFact.rawValue().toBool()) {
1175 QList<QList<QGeoCoordinate>> alternatingTransects;
1176 for (int i=0; i<transects.count(); i++) {
1177 if (!(i & 1)) {
1178 alternatingTransects.append(transects[i]);
1179 }
1180 }
1181 for (int i=transects.count()-1; i>0; i--) {
1182 if (i & 1) {
1183 alternatingTransects.append(transects[i]);
1184 }
1185 }
1186 transects = alternatingTransects;
1187 }
1188
1189 // Adjust to lawnmower pattern
1190 bool reverseVertices = false;
1191 for (int i=0; i<transects.count(); i++) {
1192 // We must reverse the vertices for every other transect in order to make a lawnmower pattern
1193 QList<QGeoCoordinate> transectVertices = transects[i];
1194 if (reverseVertices) {
1195 reverseVertices = false;
1196 QList<QGeoCoordinate> reversedVertices;
1197 for (int j=transectVertices.count()-1; j>=0; j--) {
1198 reversedVertices.append(transectVertices[j]);
1199 }
1200 transectVertices = reversedVertices;
1201 } else {
1202 reverseVertices = true;
1203 }
1204 transects[i] = transectVertices;
1205 }
1206
1207 // Convert to CoordInfo transects and append to _transects
1208 for (const QList<QGeoCoordinate>& transect: transects) {
1209 QGeoCoordinate coord;
1210 QList<TransectStyleComplexItem::CoordInfo_t> coordInfoTransect;
1212
1213 coordInfo = { transect[0], CoordTypeSurveyEntry };
1214 coordInfoTransect.append(coordInfo);
1215 coordInfo = { transect[1], CoordTypeSurveyExit };
1216 coordInfoTransect.append(coordInfo);
1217
1218 // For hover and capture we need points for each camera location within the transect
1220 double transectLength = transect[0].distanceTo(transect[1]);
1221 double transectAzimuth = transect[0].azimuthTo(transect[1]);
1222 if (triggerDistance() < transectLength) {
1223 int cInnerHoverPoints = static_cast<int>(floor(transectLength / triggerDistance()));
1224 qCDebug(SurveyComplexItemLog) << "cInnerHoverPoints" << cInnerHoverPoints;
1225 for (int i=0; i<cInnerHoverPoints; i++) {
1226 QGeoCoordinate hoverCoord = transect[0].atDistanceAndAzimuth(triggerDistance() * (i + 1), transectAzimuth);
1228 coordInfoTransect.insert(1 + i, hoverCoordInfo);
1229 }
1230 }
1231 }
1232
1233 // Extend the transect ends for turnaround
1234 if (_hasTurnaround()) {
1235 QGeoCoordinate turnaroundCoord;
1237
1238 double azimuth = transect[0].azimuthTo(transect[1]);
1239 turnaroundCoord = transect[0].atDistanceAndAzimuth(-turnAroundDistance, azimuth);
1240 turnaroundCoord.setAltitude(qQNaN());
1241 TransectStyleComplexItem::CoordInfo_t turnaroundCoordInfo = { turnaroundCoord, CoordTypeTurnaround };
1242 coordInfoTransect.prepend(turnaroundCoordInfo);
1243
1244 azimuth = transect.last().azimuthTo(transect[transect.count() - 2]);
1245 turnaroundCoord = transect.last().atDistanceAndAzimuth(-turnAroundDistance, azimuth);
1246 turnaroundCoord.setAltitude(qQNaN());
1247 coordInfo = { turnaroundCoord, CoordTypeTurnaround };
1248 coordInfoTransect.append(coordInfo);
1249 }
1250
1251 _transects.append(coordInfoTransect);
1252 }
1253 qCDebug(SurveyComplexItemLog) << "_transects.size() " << _transects.size();
1254}
1255
1256void SurveyComplexItem::_recalcCameraShots(void)
1257{
1258 double triggerDistance = this->triggerDistance();
1259
1260 if (triggerDistance == 0) {
1261 _cameraShots = 0;
1262 } else {
1265 } else {
1266 _cameraShots = 0;
1267
1269 // We have to do it the hard way based on the mission items themselves
1270 if (hoverAndCaptureEnabled()) {
1271 // Count the number of camera triggers in the mission items
1272 for (const MissionItem* missionItem: _loadedMissionItems) {
1273 _cameraShots += missionItem->command() == MAV_CMD_IMAGE_START_CAPTURE ? 1 : 0;
1274 }
1275 } else {
1276 bool waitingForTriggerStop = false;
1277 QGeoCoordinate distanceStartCoord;
1278 QGeoCoordinate distanceEndCoord;
1279 for (const MissionItem* missionItem: _loadedMissionItems) {
1280 if (missionItem->command() == MAV_CMD_NAV_WAYPOINT) {
1281 if (waitingForTriggerStop) {
1282 distanceEndCoord = QGeoCoordinate(missionItem->param5(), missionItem->param6());
1283 } else {
1284 distanceStartCoord = QGeoCoordinate(missionItem->param5(), missionItem->param6());
1285 }
1286 } else if (missionItem->command() == MAV_CMD_DO_SET_CAM_TRIGG_DIST) {
1287 if (missionItem->param1() > 0) {
1288 // Trigger start
1289 waitingForTriggerStop = true;
1290 } else {
1291 // Trigger stop
1292 waitingForTriggerStop = false;
1293 _cameraShots += qCeil(distanceEndCoord.distanceTo(distanceStartCoord) / triggerDistance);
1294 distanceStartCoord = QGeoCoordinate();
1295 distanceEndCoord = QGeoCoordinate();
1296 }
1297 }
1298 }
1299
1300 }
1301 } else {
1302 // We have transects available, calc from those
1303 for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
1304 QGeoCoordinate firstCameraCoord, lastCameraCoord;
1305 if (_hasTurnaround() && !hoverAndCaptureEnabled()) {
1306 firstCameraCoord = transect[1].coord;
1307 lastCameraCoord = transect[transect.count() - 2].coord;
1308 } else {
1309 firstCameraCoord = transect.first().coord;
1310 lastCameraCoord = transect.last().coord;
1311 }
1312 _cameraShots += qCeil(firstCameraCoord.distanceTo(lastCameraCoord) / triggerDistance);
1313 }
1314 }
1315 }
1316 }
1317
1318 emit cameraShotsChanged();
1319}
1320
1325
1327{
1328 if (_entryPoint == EntryLocationLast) {
1329 _entryPoint = EntryLocationFirst;
1330 } else {
1331 _entryPoint++;
1332 }
1333
1335
1336 setDirty(true);
1337}
1338
1340{
1341 return _vehicleSpeed == 0 ? 0 : triggerDistance() / _vehicleSpeed;
1342}
1343
1345{
1346 double hoverTime = 0;
1347
1348 if (hoverAndCaptureEnabled()) {
1349 for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
1350 hoverTime += _hoverAndCaptureDelaySeconds * transect.count();
1351 }
1352 }
1353
1354 return hoverTime;
1355}
1356
1357void SurveyComplexItem::_updateWizardMode(void)
1358{
1360 setWizardMode(false);
1361 }
1362}
#define qgcApp()
QString errorString
Geographic coordinate conversion utilities using GeographicLib.
#define QGC_LOGGING_CATEGORY(name, categoryStr)
void _setCameraNameFromV3TransectLoad(const QString &cameraName)
void setDistanceMode(QGroundControlQmlGlobal::AltitudeFrame altFrame)
Fact * imageDensity(void)
Definition CameraCalc.h:54
Fact * adjustedFootprintSide(void)
Definition CameraCalc.h:57
Fact * sideOverlap(void)
Definition CameraCalc.h:56
Fact * distanceToSurface(void)
Definition CameraCalc.h:53
Fact * adjustedFootprintFrontal(void)
Definition CameraCalc.h:58
void setCameraBrand(const QString &cameraBrand)
Fact * valueSetIsDistance(void)
Definition CameraCalc.h:52
Fact * frontalOverlap(void)
Definition CameraCalc.h:55
static QString canonicalManualCameraName(void)
SettingsFact * minTriggerInterval(void)
Definition CameraSpec.h:31
SettingsFact * imageHeight(void)
Definition CameraSpec.h:27
SettingsFact * sensorWidth(void)
Definition CameraSpec.h:24
SettingsFact * fixedOrientation(void)
Definition CameraSpec.h:30
SettingsFact * focalLength(void)
Definition CameraSpec.h:28
SettingsFact * sensorHeight(void)
Definition CameraSpec.h:25
SettingsFact * imageWidth(void)
Definition CameraSpec.h:26
SettingsFact * landscape(void)
Definition CameraSpec.h:29
void _savePresetJson(const QString &name, QJsonObject &presetObject)
static constexpr const char * jsonComplexItemTypeKey
This mission item attribute specifies the type of the complex item.
QJsonObject _loadPresetJson(const QString &name)
Holds the meta data associated with a Fact.
void setRawValue(const QVariant &value)
Definition Fact.cc:128
QVariant rawValue() const
Value after translation.
Definition Fact.h:85
void valueChanged(const QVariant &value)
This signal is only meant for use by the QT property system. It should not be connected to by client ...
Master controller for mission, fence, rally.
QmlObjectListModel & pathModel(void)
bool isValid(void) const
bool traceMode(void) const
QGeoCoordinate center(void) const
void traceModeChanged(bool traceMode)
void saveToJson(QJsonObject &json)
static constexpr const char * jsonPolygonKey
Q_INVOKABLE void clear(void)
bool isValidChanged(void)
int count(void) const
bool loadFromJson(const QJsonObject &json, bool required, QString &errorString)
This is a QGeoCoordinate within a QObject such that it can be used on a QmlObjectListModel.
T value(int index) const
Provides access to group of settings.
static SettingsManager * instance()
double timeBetweenShots(void) final
static constexpr const char * jsonComplexItemTypeValue
Q_INVOKABLE void rotateEntryPoint(void)
void refly90DegreesChanged(bool refly90Degrees)
ReadyForSaveState readyForSaveState(void) const final
double additionalTimeDelay(void) const final
bool load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString) final
void applyPreviousAltitudeFrame(QGroundControlQmlGlobal::AltitudeFrame prevAltFrame, double prevAltitude) final
static constexpr const char * jsonV3ComplexItemTypeValue
void loadPreset(const QString &name)
void savePreset(const QString &name)
void save(QJsonArray &planItems) final
static constexpr double _minimumTransectSpacingMeters
QObject * _loadedMissionItemsParent
Parent for all items in _loadedMissionItems for simpler delete.
QList< QList< CoordInfo_t > > _transects
QList< MissionItem * > _loadedMissionItems
Mission items loaded from plan file.
void setSequenceNumber(int sequenceNumber) final
QGeoCoordinate coordinate(void) const final
int sequenceNumber(void) const final
ReadyForSaveState readyForSaveState(void) const override
bool _load(const QJsonObject &complexObject, bool forPresets, QString &errorString)
static constexpr double _forceLargeTransectSpacingMeters
static constexpr int _hoverAndCaptureDelaySeconds
void _save(QJsonObject &saveObject)
@ CoordTypeTurnaround
Turnaround extension waypoint.
@ CoordTypeSurveyExit
Waypoint at exit edge of survey polygon.
@ CoordTypeInteriorHoverTrigger
Interior waypoint for hover and capture trigger.
@ CoordTypeSurveyEntry
Waypoint at entry edge of survey polygon.
static constexpr const char * jsonTypeComplexItemValue
Item type is Complex Item.
void setWizardMode(bool wizardMode)
static constexpr const char * jsonTypeKey
Json file attribute which specifies the item type.
double azimuth(void) const
bool validateKeys(const QJsonObject &jsonObject, const QList< KeyValidateInfo > &keyInfo, QString &errorString)
Validates that all required keys are present and that listed keys have the expected type.
constexpr const char * jsonVersionKey
Definition JsonParsing.h:12
void convertGeoToNed(const QGeoCoordinate &coord, const QGeoCoordinate &origin, double &x, double &y, double &z)
Definition QGCGeo.cc:34
void convertNedToGeo(double x, double y, double z, const QGeoCoordinate &origin, QGeoCoordinate &coord)
Definition QGCGeo.cc:56
void showAppMessage(const QString &message, const QString &title)
Modal application message. Queued if the UI isn't ready yet.
Definition AppMessages.cc:9