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