14#include <QtGui/QPolygonF>
15#include <QtCore/QJsonArray>
16#include <QtCore/QLineF>
23 , _gridAngleFact (settingsGroup, _metaDataMap[gridAngleName])
24 , _flyAlternateTransectsFact(settingsGroup, _metaDataMap[flyAlternateTransectsName])
25 , _splitConcavePolygonsFact (settingsGroup, _metaDataMap[splitConcavePolygonsName])
26 , _entryPoint (EntryLocationTopLeft)
28 _editorQml =
"qrc:/qml/QGroundControl/PlanView/SurveyItemEditor.qml";
30 if (_controllerVehicle && !(_controllerVehicle->fixedWing() || _controllerVehicle->vtol())) {
32 _flyAlternateTransectsFact.setRawValue(
false);
36 if (_cameraCalc.isManualCamera() || !_cameraCalc.valueSetIsDistance()->rawValue().toBool()) {
37 _cameraCalc.distanceToSurface()->setRawValue(
SettingsManager::instance()->appSettings()->defaultMissionItemAltitude()->rawValue());
53 if (!kmlOrShpFile.isEmpty()) {
54 _surveyAreaPolygon.loadKMLOrSHPFile(kmlOrShpFile);
55 _surveyAreaPolygon.setDirty(
false);
62 QJsonObject saveObject;
64 _saveCommon(saveObject);
65 planItems.append(saveObject);
70 QJsonObject saveObject;
72 _saveCommon(saveObject);
76void SurveyComplexItem::_saveCommon(QJsonObject& saveObject)
83 saveObject[_jsonGridAngleKey] = _gridAngleFact.
rawValue().toDouble();
84 saveObject[_jsonFlyAlternateTransectsKey] = _flyAlternateTransectsFact.
rawValue().toBool();
85 saveObject[_jsonSplitConcavePolygonsKey] = _splitConcavePolygonsFact.
rawValue().toBool();
86 saveObject[_jsonEntryPointKey] = _entryPoint;
97 if (!_loadV4V5(presetObject, 0,
errorString, 5,
true )) {
105 Q_UNUSED(prevAltitude);
112 QList<JsonParsing::KeyValidateInfo> versionKeyInfoList = {
120 if (version < 2 || version > 5) {
121 errorString = tr(
"Survey items do not support version %1").arg(version);
125 if (version == 4 || version == 5) {
133 _recalcCameraShots();
137 QJsonObject v3ComplexObject = complexObject;
156bool SurveyComplexItem::_loadV4V5(
const QJsonObject& complexObject,
int sequenceNumber, QString&
errorString,
int version,
bool forPresets)
158 QList<JsonParsing::KeyValidateInfo> keyInfoList = {
161 { _jsonEntryPointKey, QJsonValue::Double,
true },
162 { _jsonGridAngleKey, QJsonValue::Double,
true },
163 { _jsonFlyAlternateTransectsKey, QJsonValue::Bool,
false },
168 keyInfoList.append(jSplitPolygon);
178 errorString = tr(
"%1 does not support loading this complex mission item type: %2:%3").arg(
qgcApp()->applicationName()).arg(itemType).arg(complexType);
198 _gridAngleFact.
setRawValue (complexObject[_jsonGridAngleKey].toDouble());
199 _flyAlternateTransectsFact.
setRawValue (complexObject[_jsonFlyAlternateTransectsKey].toBool(
false));
202 _splitConcavePolygonsFact.
setRawValue (complexObject[_jsonSplitConcavePolygonsKey].toBool(
true));
205 _entryPoint = complexObject[_jsonEntryPointKey].toInt();
212bool SurveyComplexItem::_loadV3(
const QJsonObject& complexObject,
int sequenceNumber, QString&
errorString)
214 QList<JsonParsing::KeyValidateInfo> mainKeyInfoList = {
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 },
234 errorString = tr(
"%1 does not support loading this complex mission item type: %2:%3").arg(
qgcApp()->applicationName()).arg(itemType).arg(complexType);
249 bool manualGrid = complexObject[_jsonV3ManualGridKey].toBool(
true);
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 },
259 QJsonObject gridObject = complexObject[_jsonV3GridObjectKey].toObject();
265 _gridAngleFact.
setRawValue (gridObject[_jsonV3GridAngleKey].toDouble());
268 if (gridObject.contains(_jsonEntryPointKey)) {
269 _entryPoint = gridObject[_jsonEntryPointKey].toInt();
281 if (!complexObject.contains(_jsonV3CameraObjectKey)) {
282 errorString = tr(
"%1 but %2 object is missing").arg(
"manualGrid = false").arg(
"camera");
287 QJsonObject cameraObject = complexObject[_jsonV3CameraObjectKey].toObject();
290 QString incorrectImageSideOverlap =
"imageSizeOverlap";
291 if (cameraObject.contains(incorrectImageSideOverlap)) {
292 cameraObject[_jsonV3SideOverlapKey] = cameraObject[incorrectImageSideOverlap];
293 cameraObject.remove(incorrectImageSideOverlap);
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 },
346void SurveyComplexItem::_reverseTransectOrder(QList<QList<QGeoCoordinate>>& transects)
348 QList<QList<QGeoCoordinate>> rgReversedTransects;
349 for (
int i=transects.count() - 1; i>=0; i--) {
350 rgReversedTransects.append(transects[i]);
352 transects = rgReversedTransects;
356void SurveyComplexItem::_reverseInternalTransectPoints(QList<QList<QGeoCoordinate>>& transects)
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]);
364 transects[i] = rgReversedCoords;
372void SurveyComplexItem::_optimizeTransectsForShortestDistance(
const QGeoCoordinate& distanceCoord, QList<QList<QGeoCoordinate>>& transects)
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);
380 int shortestIndex = 0;
381 double shortestDistance = rgTransectDistance[0];
382 for (
int i=1; i<3; i++) {
383 if (rgTransectDistance[i] < shortestDistance) {
385 shortestDistance = rgTransectDistance[i];
389 if (shortestIndex > 1) {
391 _reverseTransectOrder(transects);
393 if (shortestIndex & 1) {
395 _reverseInternalTransectPoints(transects);
399qreal SurveyComplexItem::_ccw(QPointF pt1, QPointF pt2, QPointF pt3)
401 return (pt2.x()-pt1.x())*(pt3.y()-pt1.y()) - (pt2.y()-pt1.y())*(pt3.x()-pt1.x());
404qreal SurveyComplexItem::_dp(QPointF pt1, QPointF pt2)
406 return (pt2.x()-pt1.x())/qSqrt((pt2.x()-pt1.x())*(pt2.x()-pt1.x()) + (pt2.y()-pt1.y())*(pt2.y()-pt1.y()));
409void SurveyComplexItem::_swapPoints(QList<QPointF>& points,
int index1,
int index2)
411 QPointF temp = points[index1];
412 points[index1] = points[index2];
413 points[index2] = temp;
417bool SurveyComplexItem::_gridAngleIsNorthSouthTransects()
424void SurveyComplexItem::_adjustTransectsToEntryPointLocation(QList<QList<QGeoCoordinate>>& transects)
426 if (transects.count() == 0) {
430 bool reversePoints =
false;
431 bool reverseTransects =
false;
434 reversePoints =
true;
437 reverseTransects =
true;
441 qCDebug(SurveyComplexItemLog) <<
"_adjustTransectsToEntryPointLocation Reverse Points";
442 _reverseInternalTransectPoints(transects);
444 if (reverseTransects) {
445 qCDebug(SurveyComplexItemLog) <<
"_adjustTransectsToEntryPointLocation Reverse Transects";
446 _reverseTransectOrder(transects);
449 qCDebug(SurveyComplexItemLog) <<
"_adjustTransectsToEntryPointLocation Modified entry point:entryLocation" << transects.first().first() << _entryPoint;
452QPointF SurveyComplexItem::_rotatePoint(
const QPointF& point,
const QPointF& origin,
double angle)
455 double radians = (M_PI / 180.0) * -angle;
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());
463void SurveyComplexItem::_intersectLinesWithRect(
const QList<QLineF>& lineList,
const QRectF& boundRect, QList<QLineF>& resultLines)
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());
470 for (
int i=0; i<lineList.count(); i++) {
471 QPointF intersectPoint;
472 QLineF intersectLine;
473 const QLineF& line = lineList[i];
475 auto isLineBoundedIntersect = [&line, &intersectPoint](
const QLineF& linePosition) {
476 return line.intersects(linePosition, &intersectPoint) == QLineF::BoundedIntersection;
480 if (isLineBoundedIntersect(topLine)) {
481 intersectLine.setP1(intersectPoint);
484 if (isLineBoundedIntersect(rightLine)) {
485 if (foundCount == 0) {
486 intersectLine.setP1(intersectPoint);
488 if (foundCount != 1) {
489 qWarning() <<
"Found more than two intersecting points";
491 intersectLine.setP2(intersectPoint);
495 if (isLineBoundedIntersect(bottomLine)) {
496 if (foundCount == 0) {
497 intersectLine.setP1(intersectPoint);
499 if (foundCount != 1) {
500 qWarning() <<
"Found more than two intersecting points";
502 intersectLine.setP2(intersectPoint);
506 if (isLineBoundedIntersect(leftLine)) {
507 if (foundCount == 0) {
508 intersectLine.setP1(intersectPoint);
510 if (foundCount != 1) {
511 qWarning() <<
"Found more than two intersecting points";
513 intersectLine.setP2(intersectPoint);
518 if (foundCount == 2) {
519 resultLines += intersectLine;
524void SurveyComplexItem::_intersectLinesWithPolygon(
const QList<QLineF>& lineList,
const QPolygonF& polygon, QList<QLineF>& resultLines)
528 for (
int i=0; i<lineList.count(); i++) {
529 const QLineF& line = lineList[i];
530 QList<QPointF> intersections;
533 for (
int j=0; j<polygon.count()-1; j++) {
534 QPointF intersectPoint;
535 QLineF polygonLine = QLineF(polygon[j], polygon[j+1]);
537 auto intersect = line.intersects(polygonLine, &intersectPoint);
538 if (intersect == QLineF::BoundedIntersection) {
539 if (!intersections.contains(intersectPoint)) {
540 intersections.append(intersectPoint);
547 if (intersections.count() > 1) {
550 double currentMaxDistance = 0;
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]);
556 double newMaxDistance = lineTest.length();
557 if (newMaxDistance > currentMaxDistance) {
558 firstPoint = intersections[intersectionIndex];
559 secondPoint = intersections[compareIndex];
560 currentMaxDistance = newMaxDistance;
565 resultLines += QLineF(firstPoint, secondPoint);
571void SurveyComplexItem::_adjustLineDirection(
const QList<QLineF>& lineList, QList<QLineF>& resultLines)
573 qreal firstAngle = 0;
574 for (
int i=0; i<lineList.count(); i++) {
575 const QLineF& line = lineList[i];
579 firstAngle = line.angle();
582 if (qAbs(line.angle() - firstAngle) > 1.0) {
583 adjustedLine.setP1(line.p2());
584 adjustedLine.setP2(line.p1());
589 resultLines += adjustedLine;
593double SurveyComplexItem::_clampGridAngle90(
double gridAngle)
604bool SurveyComplexItem::_nextTransectCoord(
const QList<QGeoCoordinate>& transectPoints,
int pointIndex, QGeoCoordinate& coord)
606 if (pointIndex > transectPoints.count()) {
607 qWarning() <<
"Bad grid generation";
611 coord = transectPoints[pointIndex];
615bool SurveyComplexItem::_hasTurnaround(
void)
const
620double SurveyComplexItem::_turnaroundDistance(
void)
const
625void SurveyComplexItem::_rebuildTransectsPhase1(
void)
627 _rebuildTransectsPhase1WorkerSinglePolygon(
false );
629 _rebuildTransectsPhase1WorkerSinglePolygon(
true );
633void SurveyComplexItem::_rebuildTransectsPhase1WorkerSinglePolygon(
bool refly)
652 QList<QPointF> polygonPoints;
654 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" <<
_surveyAreaPolygon.
count() << tangentOrigin;
664 polygonPoints += QPointF(x, y);
665 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
681 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Clamped grid angle" <<
gridAngle;
683 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 gridSpacing:gridAngle:refly" << gridSpacing <<
gridAngle << refly;
687 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Polygon";
689 for (
int i=0; i<polygonPoints.count(); i++) {
690 qCDebug(SurveyComplexItemLog) <<
"Vertex" << polygonPoints[i];
691 polygon << polygonPoints[i];
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();
701 QList<QLineF> lineList;
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;
714 lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter,
gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter,
gridAngle));
715 transectX += gridSpacing;
719 QList<QLineF> intersectLines;
721 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
724 intersectLines = lineList;
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);
737 lineList.append(firstLine);
738 intersectLines = lineList;
739 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
744 QList<QLineF> resultLines;
745 _adjustLineDirection(intersectLines, resultLines);
748 QList<QList<QGeoCoordinate>> transects;
749 for (
const QLineF& line : resultLines) {
750 QGeoCoordinate coord;
751 QList<QGeoCoordinate> transect;
754 transect.append(coord);
756 transect.append(coord);
758 transects.append(transect);
761 _adjustTransectsToEntryPointLocation(transects);
764 _optimizeTransectsForShortestDistance(
_transects.last().last().coord, transects);
767 if (_flyAlternateTransectsFact.
rawValue().toBool()) {
768 QList<QList<QGeoCoordinate>> alternatingTransects;
769 for (
int i=0; i<transects.count(); i++) {
771 alternatingTransects.append(transects[i]);
774 for (
int i=transects.count()-1; i>0; i--) {
776 alternatingTransects.append(transects[i]);
779 transects = alternatingTransects;
783 bool reverseVertices =
false;
784 for (
int i=0; i<transects.count(); i++) {
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]);
793 transectVertices = reversedVertices;
795 reverseVertices =
true;
797 transects[i] = transectVertices;
801 for (
const QList<QGeoCoordinate>& transect : transects) {
802 QGeoCoordinate coord;
803 QList<TransectStyleComplexItem::CoordInfo_t> coordInfoTransect;
807 coordInfoTransect.append(coordInfo);
809 coordInfoTransect.append(coordInfo);
813 double transectLength = transect[0].distanceTo(transect[1]);
814 double transectAzimuth = transect[0].azimuthTo(transect[1]);
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);
827 if (_hasTurnaround()) {
828 QGeoCoordinate turnaroundCoord;
831 double azimuth = transect[0].azimuthTo(transect[1]);
833 turnaroundCoord.setAltitude(qQNaN());
835 coordInfoTransect.prepend(turnaroundCoordInfo);
837 azimuth = transect.last().azimuthTo(transect[transect.count() - 2]);
839 turnaroundCoord.setAltitude(qQNaN());
841 coordInfoTransect.append(coordInfo);
852void SurveyComplexItem::_rebuildTransectsPhase1WorkerSplitPolygons(
bool refly)
871 QList<QPointF> polygonPoints;
873 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" <<
_surveyAreaPolygon.
count() << tangentOrigin;
883 polygonPoints += QPointF(x, y);
884 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
889 for (
int i=0; i<polygonPoints.count(); i++) {
890 qCDebug(SurveyComplexItemLog) <<
"Vertex" << polygonPoints[i];
891 polygon << polygonPoints[i];
895 QList<QPolygonF> polygons{};
896 _PolygonDecomposeConvex(polygon, polygons);
899 for (
auto p = polygons.begin(); p != polygons.end(); ++p) {
900 QPointF* vMatch =
nullptr;
902 if (p != polygons.begin()) {
905 for (
auto& j : *pLast) {
923 _rebuildTransectsFromPolygon(refly, *p, tangentOrigin, vMatch);
927void SurveyComplexItem::_PolygonDecomposeConvex(
const QPolygonF& polygon, QList<QPolygonF>& decomposedPolygons)
930 int decompSize = std::numeric_limits<int>::max();
931 if (polygon.size() < 3)
return;
932 if (polygon.size() == 3) {
933 decomposedPolygons << polygon;
937 QList<QPolygonF> decomposedPolygonsMin{};
939 for (
auto vertex = polygon.begin(); vertex != polygon.end(); ++vertex)
942 bool vertexIsReflex = _VertexIsReflex(polygon, vertex);
944 if (!vertexIsReflex)
continue;
946 for (
auto vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther)
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;
958 auto polyLeftContainsReflex =
false;
959 while ( v != vertexOther) {
960 if (v != vertex && _VertexIsReflex(polygon, v)) {
961 polyLeftContainsReflex =
true;
965 if (v == polygon.end()) v = polygon.begin();
967 polyLeft << *vertexOther;
968 auto polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3);
972 auto polyRightContainsReflex =
false;
973 while ( v != vertex) {
974 if (v != vertex && _VertexIsReflex(polygon, v)) {
975 polyRightContainsReflex =
true;
979 if (v == polygon.end()) v = polygon.begin();
981 polyRight << *vertex;
982 auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3);
984 if (!polyLeftValid || ! polyRightValid) {
990 QList<QPolygonF> polyLeftDecomposed{};
991 _PolygonDecomposeConvex(polyLeft, polyLeftDecomposed);
993 QList<QPolygonF> polyRightDecomposed{};
994 _PolygonDecomposeConvex(polyRight, polyRightDecomposed);
997 auto subSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
998 if ((polyLeftContainsReflex && polyLeftDecomposed.size() == 1)
999 || (polyRightContainsReflex && polyRightDecomposed.size() == 1))
1002 subSize = std::numeric_limits<int>::max();
1004 if (subSize < decompSize) {
1005 decompSize = subSize;
1006 decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
1013 if (decomposedPolygonsMin.size() > 0) {
1014 decomposedPolygons << decomposedPolygonsMin;
1016 decomposedPolygons << polygon;
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;
1030 bool visible =
true;
1032 QLineF lineAB{*vertexA, *vertexB};
1033 auto distanceAB = lineAB.length();
1036 for (
auto vertexC = polygon.begin(); vertexC != polygon.end(); ++vertexC)
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{};
1046 auto intersects = lineAB.intersects(lineCD, &intersection);
1047 if (intersects == QLineF::IntersectType::BoundedIntersection) {
1052 QLineF lineIntersection{*vertexA, intersection};
1053 auto distanceIntersection = lineIntersection.length();
1054 qCDebug(SurveyComplexItemLog) <<
"_VertexCanSeeOther distanceIntersection " << distanceIntersection;
1055 if (distanceIntersection < distanceAB) {
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())));
1075void SurveyComplexItem::_rebuildTransectsFromPolygon(
bool refly,
const QPolygonF& polygon,
const QGeoCoordinate& tangentOrigin,
const QPointF*
const transitionPoint)
1084 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Clamped grid angle" <<
gridAngle;
1086 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 gridSpacing:gridAngle:refly" << gridSpacing <<
gridAngle << refly;
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();
1098 QList<QLineF> lineList;
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;
1111 lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter,
gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter,
gridAngle));
1112 transectX += gridSpacing;
1116 QList<QLineF> intersectLines;
1118 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
1121 intersectLines = lineList;
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);
1134 lineList.append(firstLine);
1135 intersectLines = lineList;
1136 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
1141 QList<QLineF> resultLines;
1142 _adjustLineDirection(intersectLines, resultLines);
1145 QList<QList<QGeoCoordinate>> transects;
1147 if (transitionPoint !=
nullptr) {
1148 QList<QGeoCoordinate> transect;
1149 QGeoCoordinate coord;
1151 transect.append(coord);
1152 transect.append(coord);
1153 transects.append(transect);
1156 for (
const QLineF& line: resultLines) {
1157 QList<QGeoCoordinate> transect;
1158 QGeoCoordinate coord;
1161 transect.append(coord);
1163 transect.append(coord);
1165 transects.append(transect);
1168 _adjustTransectsToEntryPointLocation(transects);
1171 _optimizeTransectsForShortestDistance(
_transects.last().last().coord, transects);
1174 if (_flyAlternateTransectsFact.
rawValue().toBool()) {
1175 QList<QList<QGeoCoordinate>> alternatingTransects;
1176 for (
int i=0; i<transects.count(); i++) {
1178 alternatingTransects.append(transects[i]);
1181 for (
int i=transects.count()-1; i>0; i--) {
1183 alternatingTransects.append(transects[i]);
1186 transects = alternatingTransects;
1190 bool reverseVertices =
false;
1191 for (
int i=0; i<transects.count(); i++) {
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]);
1200 transectVertices = reversedVertices;
1202 reverseVertices =
true;
1204 transects[i] = transectVertices;
1208 for (
const QList<QGeoCoordinate>& transect: transects) {
1209 QGeoCoordinate coord;
1210 QList<TransectStyleComplexItem::CoordInfo_t> coordInfoTransect;
1214 coordInfoTransect.append(coordInfo);
1216 coordInfoTransect.append(coordInfo);
1220 double transectLength = transect[0].distanceTo(transect[1]);
1221 double transectAzimuth = transect[0].azimuthTo(transect[1]);
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);
1234 if (_hasTurnaround()) {
1235 QGeoCoordinate turnaroundCoord;
1238 double azimuth = transect[0].azimuthTo(transect[1]);
1240 turnaroundCoord.setAltitude(qQNaN());
1242 coordInfoTransect.prepend(turnaroundCoordInfo);
1244 azimuth = transect.last().azimuthTo(transect[transect.count() - 2]);
1246 turnaroundCoord.setAltitude(qQNaN());
1248 coordInfoTransect.append(coordInfo);
1253 qCDebug(SurveyComplexItemLog) <<
"_transects.size() " <<
_transects.size();
1256void SurveyComplexItem::_recalcCameraShots(
void)
1260 if (triggerDistance == 0) {
1273 _cameraShots += missionItem->command() == MAV_CMD_IMAGE_START_CAPTURE ? 1 : 0;
1276 bool waitingForTriggerStop =
false;
1277 QGeoCoordinate distanceStartCoord;
1278 QGeoCoordinate distanceEndCoord;
1280 if (missionItem->command() == MAV_CMD_NAV_WAYPOINT) {
1281 if (waitingForTriggerStop) {
1282 distanceEndCoord = QGeoCoordinate(missionItem->param5(), missionItem->param6());
1284 distanceStartCoord = QGeoCoordinate(missionItem->param5(), missionItem->param6());
1286 }
else if (missionItem->command() == MAV_CMD_DO_SET_CAM_TRIGG_DIST) {
1287 if (missionItem->param1() > 0) {
1289 waitingForTriggerStop =
true;
1292 waitingForTriggerStop =
false;
1294 distanceStartCoord = QGeoCoordinate();
1295 distanceEndCoord = QGeoCoordinate();
1303 for (
const QList<TransectStyleComplexItem::CoordInfo_t>& transect:
_transects) {
1304 QGeoCoordinate firstCameraCoord, lastCameraCoord;
1306 firstCameraCoord = transect[1].coord;
1307 lastCameraCoord = transect[transect.count() - 2].coord;
1309 firstCameraCoord = transect.first().coord;
1310 lastCameraCoord = transect.last().coord;
1346 double hoverTime = 0;
1349 for (
const QList<TransectStyleComplexItem::CoordInfo_t>& transect:
_transects) {
1357void SurveyComplexItem::_updateWizardMode(
void)
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)
Fact * adjustedFootprintSide(void)
Fact * distanceToSurface(void)
Fact * adjustedFootprintFrontal(void)
void setCameraBrand(const QString &cameraBrand)
Fact * valueSetIsDistance(void)
Fact * frontalOverlap(void)
static QString canonicalManualCameraName(void)
SettingsFact * minTriggerInterval(void)
SettingsFact * imageHeight(void)
SettingsFact * sensorWidth(void)
SettingsFact * fixedOrientation(void)
SettingsFact * focalLength(void)
SettingsFact * sensorHeight(void)
SettingsFact * imageWidth(void)
SettingsFact * landscape(void)
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 setRawValue(const QVariant &value)
QVariant rawValue() const
Value after translation.
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 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)
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.
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)
@ EntryLocationBottomRight
@ EntryLocationBottomLeft
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
SettingsFact _refly90DegreesFact
bool hoverAndCaptureEnabled(void) const
QList< MissionItem * > _loadedMissionItems
Mission items loaded from plan file.
void _recalcComplexDistance(void)
void setSequenceNumber(int sequenceNumber) final
QGeoCoordinate coordinate(void) const final
int sequenceNumber(void) const final
bool triggerCamera(void) const
ReadyForSaveState readyForSaveState(void) const override
double _turnAroundDistance(void) const
bool _load(const QJsonObject &complexObject, bool forPresets, QString &errorString)
static constexpr double _forceLargeTransectSpacingMeters
static constexpr int _hoverAndCaptureDelaySeconds
QGCMapPolygon _surveyAreaPolygon
Fact * turnAroundDistance(void)
SettingsFact _hoverAndCaptureFact
SettingsFact _cameraTriggerInTurnAroundFact
double triggerDistance(void) const
void cameraShotsChanged(void)
void _save(QJsonObject &saveObject)
void _rebuildTransects(void)
SettingsFact _turnAroundDistanceFact
void setDirty(bool dirty) final
@ 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
void convertGeoToNed(const QGeoCoordinate &coord, const QGeoCoordinate &origin, double &x, double &y, double &z)
void convertNedToGeo(double x, double y, double z, const QGeoCoordinate &origin, QGeoCoordinate &coord)
void showAppMessage(const QString &message, const QString &title)
Modal application message. Queued if the UI isn't ready yet.