13#include <QtGui/QPolygonF>
14#include <QtCore/QJsonArray>
15#include <QtCore/QLineF>
24 , _gridAngleFact (settingsGroup, _metaDataMap[gridAngleName])
25 , _flyAlternateTransectsFact(settingsGroup, _metaDataMap[flyAlternateTransectsName])
26 , _splitConcavePolygonsFact (settingsGroup, _metaDataMap[splitConcavePolygonsName])
27 , _entryPoint (EntryLocationTopLeft)
29 _editorQml =
"qrc:/qml/QGroundControl/Controls/SurveyItemEditor.qml";
31 if (_controllerVehicle && !(_controllerVehicle->fixedWing() || _controllerVehicle->vtol())) {
33 _flyAlternateTransectsFact.setRawValue(
false);
37 if (_cameraCalc.isManualCamera() || !_cameraCalc.valueSetIsDistance()->rawValue().toBool()) {
38 _cameraCalc.distanceToSurface()->setRawValue(SettingsManager::instance()->appSettings()->defaultMissionItemAltitude()->rawValue());
54 if (!kmlOrShpFile.isEmpty()) {
55 _surveyAreaPolygon.loadKMLOrSHPFile(kmlOrShpFile);
56 _surveyAreaPolygon.setDirty(
false);
63 QJsonObject saveObject;
65 _saveCommon(saveObject);
66 planItems.append(saveObject);
71 QJsonObject saveObject;
73 _saveCommon(saveObject);
77void SurveyComplexItem::_saveCommon(QJsonObject& saveObject)
84 saveObject[_jsonGridAngleKey] = _gridAngleFact.rawValue().toDouble();
85 saveObject[_jsonFlyAlternateTransectsKey] = _flyAlternateTransectsFact.rawValue().toBool();
86 saveObject[_jsonSplitConcavePolygonsKey] = _splitConcavePolygonsFact.rawValue().toBool();
87 saveObject[_jsonEntryPointKey] = _entryPoint;
98 if (!_loadV4V5(presetObject, 0,
errorString, 5,
true )) {
99 qgcApp()->showAppMessage(QStringLiteral(
"Internal Error: Preset load failed. Name: %1 Error: %2").arg(presetName).arg(
errorString));
107 QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
115 if (version < 2 || version > 5) {
116 errorString = tr(
"Survey items do not support version %1").arg(version);
120 if (version == 4 || version == 5) {
128 _recalcCameraShots();
132 QJsonObject v3ComplexObject = complexObject;
151bool SurveyComplexItem::_loadV4V5(
const QJsonObject& complexObject,
int sequenceNumber, QString&
errorString,
int version,
bool forPresets)
153 QList<JsonHelper::KeyValidateInfo> keyInfoList = {
156 { _jsonEntryPointKey, QJsonValue::Double,
true },
157 { _jsonGridAngleKey, QJsonValue::Double,
true },
158 { _jsonFlyAlternateTransectsKey, QJsonValue::Bool,
false },
163 keyInfoList.append(jSplitPolygon);
173 errorString = tr(
"%1 does not support loading this complex mission item type: %2:%3").arg(
qgcApp()->applicationName()).arg(itemType).arg(complexType);
193 _gridAngleFact.setRawValue (complexObject[_jsonGridAngleKey].toDouble());
194 _flyAlternateTransectsFact.setRawValue (complexObject[_jsonFlyAlternateTransectsKey].toBool(
false));
197 _splitConcavePolygonsFact.setRawValue (complexObject[_jsonSplitConcavePolygonsKey].toBool(
true));
200 _entryPoint = complexObject[_jsonEntryPointKey].toInt();
207bool SurveyComplexItem::_loadV3(
const QJsonObject& complexObject,
int sequenceNumber, QString&
errorString)
209 QList<JsonHelper::KeyValidateInfo> mainKeyInfoList = {
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 },
229 errorString = tr(
"%1 does not support loading this complex mission item type: %2:%3").arg(
qgcApp()->applicationName()).arg(itemType).arg(complexType);
244 bool manualGrid = complexObject[_jsonV3ManualGridKey].toBool(
true);
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 },
254 QJsonObject gridObject = complexObject[_jsonV3GridObjectKey].toObject();
260 _gridAngleFact.setRawValue (gridObject[_jsonV3GridAngleKey].toDouble());
263 if (gridObject.contains(_jsonEntryPointKey)) {
264 _entryPoint = gridObject[_jsonEntryPointKey].toInt();
276 if (!complexObject.contains(_jsonV3CameraObjectKey)) {
277 errorString = tr(
"%1 but %2 object is missing").arg(
"manualGrid = false").arg(
"camera");
282 QJsonObject cameraObject = complexObject[_jsonV3CameraObjectKey].toObject();
285 QString incorrectImageSideOverlap =
"imageSizeOverlap";
286 if (cameraObject.contains(incorrectImageSideOverlap)) {
287 cameraObject[_jsonV3SideOverlapKey] = cameraObject[incorrectImageSideOverlap];
288 cameraObject.remove(incorrectImageSideOverlap);
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 },
309 _cameraCalc.
landscape()->setRawValue (cameraObject[_jsonV3CameraOrientationLandscapeKey].toBool(
true));
312 _cameraCalc.sensorWidth()->setRawValue (cameraObject[_jsonV3CameraSensorWidthKey].toDouble());
341void SurveyComplexItem::_reverseTransectOrder(QList<QList<QGeoCoordinate>>& transects)
343 QList<QList<QGeoCoordinate>> rgReversedTransects;
344 for (
int i=transects.count() - 1; i>=0; i--) {
345 rgReversedTransects.append(transects[i]);
347 transects = rgReversedTransects;
351void SurveyComplexItem::_reverseInternalTransectPoints(QList<QList<QGeoCoordinate>>& transects)
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]);
359 transects[i] = rgReversedCoords;
367void SurveyComplexItem::_optimizeTransectsForShortestDistance(
const QGeoCoordinate& distanceCoord, QList<QList<QGeoCoordinate>>& transects)
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);
375 int shortestIndex = 0;
376 double shortestDistance = rgTransectDistance[0];
377 for (
int i=1; i<3; i++) {
378 if (rgTransectDistance[i] < shortestDistance) {
380 shortestDistance = rgTransectDistance[i];
384 if (shortestIndex > 1) {
386 _reverseTransectOrder(transects);
388 if (shortestIndex & 1) {
390 _reverseInternalTransectPoints(transects);
394qreal SurveyComplexItem::_ccw(QPointF pt1, QPointF pt2, QPointF pt3)
396 return (pt2.x()-pt1.x())*(pt3.y()-pt1.y()) - (pt2.y()-pt1.y())*(pt3.x()-pt1.x());
399qreal SurveyComplexItem::_dp(QPointF pt1, QPointF pt2)
401 return (pt2.x()-pt1.x())/qSqrt((pt2.x()-pt1.x())*(pt2.x()-pt1.x()) + (pt2.y()-pt1.y())*(pt2.y()-pt1.y()));
404void SurveyComplexItem::_swapPoints(QList<QPointF>& points,
int index1,
int index2)
406 QPointF temp = points[index1];
407 points[index1] = points[index2];
408 points[index2] = temp;
412bool SurveyComplexItem::_gridAngleIsNorthSouthTransects()
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);
419void SurveyComplexItem::_adjustTransectsToEntryPointLocation(QList<QList<QGeoCoordinate>>& transects)
421 if (transects.count() == 0) {
425 bool reversePoints =
false;
426 bool reverseTransects =
false;
429 reversePoints =
true;
432 reverseTransects =
true;
436 qCDebug(SurveyComplexItemLog) <<
"_adjustTransectsToEntryPointLocation Reverse Points";
437 _reverseInternalTransectPoints(transects);
439 if (reverseTransects) {
440 qCDebug(SurveyComplexItemLog) <<
"_adjustTransectsToEntryPointLocation Reverse Transects";
441 _reverseTransectOrder(transects);
444 qCDebug(SurveyComplexItemLog) <<
"_adjustTransectsToEntryPointLocation Modified entry point:entryLocation" << transects.first().first() << _entryPoint;
447QPointF SurveyComplexItem::_rotatePoint(
const QPointF& point,
const QPointF& origin,
double angle)
450 double radians = (M_PI / 180.0) * -angle;
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());
458void SurveyComplexItem::_intersectLinesWithRect(
const QList<QLineF>& lineList,
const QRectF& boundRect, QList<QLineF>& resultLines)
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());
465 for (
int i=0; i<lineList.count(); i++) {
466 QPointF intersectPoint;
467 QLineF intersectLine;
468 const QLineF& line = lineList[i];
470 auto isLineBoundedIntersect = [&line, &intersectPoint](
const QLineF& linePosition) {
471 return line.intersects(linePosition, &intersectPoint) == QLineF::BoundedIntersection;
475 if (isLineBoundedIntersect(topLine)) {
476 intersectLine.setP1(intersectPoint);
479 if (isLineBoundedIntersect(rightLine)) {
480 if (foundCount == 0) {
481 intersectLine.setP1(intersectPoint);
483 if (foundCount != 1) {
484 qWarning() <<
"Found more than two intersecting points";
486 intersectLine.setP2(intersectPoint);
490 if (isLineBoundedIntersect(bottomLine)) {
491 if (foundCount == 0) {
492 intersectLine.setP1(intersectPoint);
494 if (foundCount != 1) {
495 qWarning() <<
"Found more than two intersecting points";
497 intersectLine.setP2(intersectPoint);
501 if (isLineBoundedIntersect(leftLine)) {
502 if (foundCount == 0) {
503 intersectLine.setP1(intersectPoint);
505 if (foundCount != 1) {
506 qWarning() <<
"Found more than two intersecting points";
508 intersectLine.setP2(intersectPoint);
513 if (foundCount == 2) {
514 resultLines += intersectLine;
519void SurveyComplexItem::_intersectLinesWithPolygon(
const QList<QLineF>& lineList,
const QPolygonF& polygon, QList<QLineF>& resultLines)
523 for (
int i=0; i<lineList.count(); i++) {
524 const QLineF& line = lineList[i];
525 QList<QPointF> intersections;
528 for (
int j=0; j<polygon.count()-1; j++) {
529 QPointF intersectPoint;
530 QLineF polygonLine = QLineF(polygon[j], polygon[j+1]);
532 auto intersect = line.intersects(polygonLine, &intersectPoint);
533 if (intersect == QLineF::BoundedIntersection) {
534 if (!intersections.contains(intersectPoint)) {
535 intersections.append(intersectPoint);
542 if (intersections.count() > 1) {
545 double currentMaxDistance = 0;
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]);
551 double newMaxDistance = lineTest.length();
552 if (newMaxDistance > currentMaxDistance) {
553 firstPoint = intersections[intersectionIndex];
554 secondPoint = intersections[compareIndex];
555 currentMaxDistance = newMaxDistance;
560 resultLines += QLineF(firstPoint, secondPoint);
566void SurveyComplexItem::_adjustLineDirection(
const QList<QLineF>& lineList, QList<QLineF>& resultLines)
568 qreal firstAngle = 0;
569 for (
int i=0; i<lineList.count(); i++) {
570 const QLineF& line = lineList[i];
574 firstAngle = line.angle();
577 if (qAbs(line.angle() - firstAngle) > 1.0) {
578 adjustedLine.setP1(line.p2());
579 adjustedLine.setP2(line.p1());
584 resultLines += adjustedLine;
588double SurveyComplexItem::_clampGridAngle90(
double gridAngle)
591 if (gridAngle > 90.0) {
593 }
else if (gridAngle < -90.0) {
599bool SurveyComplexItem::_nextTransectCoord(
const QList<QGeoCoordinate>& transectPoints,
int pointIndex, QGeoCoordinate& coord)
601 if (pointIndex > transectPoints.count()) {
602 qWarning() <<
"Bad grid generation";
606 coord = transectPoints[pointIndex];
610bool SurveyComplexItem::_hasTurnaround(
void)
const
615double SurveyComplexItem::_turnaroundDistance(
void)
const
620void SurveyComplexItem::_rebuildTransectsPhase1(
void)
622 _rebuildTransectsPhase1WorkerSinglePolygon(
false );
624 _rebuildTransectsPhase1WorkerSinglePolygon(
true );
628void SurveyComplexItem::_rebuildTransectsPhase1WorkerSinglePolygon(
bool refly)
647 QList<QPointF> polygonPoints;
649 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" <<
_surveyAreaPolygon.
count() << tangentOrigin;
659 polygonPoints += QPointF(x, y);
660 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
665 double gridAngle = _gridAngleFact.rawValue().toDouble();
674 gridAngle = _clampGridAngle90(gridAngle);
675 gridAngle += refly ? 90 : 0;
676 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Clamped grid angle" << gridAngle;
678 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly;
682 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Polygon";
684 for (
int i=0; i<polygonPoints.count(); i++) {
685 qCDebug(SurveyComplexItemLog) <<
"Vertex" << polygonPoints[i];
686 polygon << polygonPoints[i];
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();
696 QList<QLineF> lineList;
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;
709 lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle));
710 transectX += gridSpacing;
714 QList<QLineF> intersectLines;
716 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
719 intersectLines = lineList;
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);
732 lineList.append(firstLine);
733 intersectLines = lineList;
734 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
739 QList<QLineF> resultLines;
740 _adjustLineDirection(intersectLines, resultLines);
743 QList<QList<QGeoCoordinate>> transects;
744 for (
const QLineF& line : resultLines) {
745 QGeoCoordinate coord;
746 QList<QGeoCoordinate> transect;
749 transect.append(coord);
751 transect.append(coord);
753 transects.append(transect);
756 _adjustTransectsToEntryPointLocation(transects);
759 _optimizeTransectsForShortestDistance(
_transects.last().last().coord, transects);
762 if (_flyAlternateTransectsFact.rawValue().toBool()) {
763 QList<QList<QGeoCoordinate>> alternatingTransects;
764 for (
int i=0; i<transects.count(); i++) {
766 alternatingTransects.append(transects[i]);
769 for (
int i=transects.count()-1; i>0; i--) {
771 alternatingTransects.append(transects[i]);
774 transects = alternatingTransects;
778 bool reverseVertices =
false;
779 for (
int i=0; i<transects.count(); i++) {
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]);
788 transectVertices = reversedVertices;
790 reverseVertices =
true;
792 transects[i] = transectVertices;
796 for (
const QList<QGeoCoordinate>& transect : transects) {
797 QGeoCoordinate coord;
798 QList<TransectStyleComplexItem::CoordInfo_t> coordInfoTransect;
802 coordInfoTransect.append(coordInfo);
804 coordInfoTransect.append(coordInfo);
808 double transectLength = transect[0].distanceTo(transect[1]);
809 double transectAzimuth = transect[0].azimuthTo(transect[1]);
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);
822 if (_hasTurnaround()) {
823 QGeoCoordinate turnaroundCoord;
826 double azimuth = transect[0].azimuthTo(transect[1]);
828 turnaroundCoord.setAltitude(qQNaN());
830 coordInfoTransect.prepend(turnaroundCoordInfo);
832 azimuth = transect.last().azimuthTo(transect[transect.count() - 2]);
834 turnaroundCoord.setAltitude(qQNaN());
836 coordInfoTransect.append(coordInfo);
847void SurveyComplexItem::_rebuildTransectsPhase1WorkerSplitPolygons(
bool refly)
866 QList<QPointF> polygonPoints;
868 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" <<
_surveyAreaPolygon.
count() << tangentOrigin;
878 polygonPoints += QPointF(x, y);
879 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
884 for (
int i=0; i<polygonPoints.count(); i++) {
885 qCDebug(SurveyComplexItemLog) <<
"Vertex" << polygonPoints[i];
886 polygon << polygonPoints[i];
890 QList<QPolygonF> polygons{};
891 _PolygonDecomposeConvex(polygon, polygons);
894 for (
auto p = polygons.begin(); p != polygons.end(); ++p) {
895 QPointF* vMatch =
nullptr;
897 if (p != polygons.begin()) {
900 for (
auto& j : *pLast) {
918 _rebuildTransectsFromPolygon(refly, *p, tangentOrigin, vMatch);
922void SurveyComplexItem::_PolygonDecomposeConvex(
const QPolygonF& polygon, QList<QPolygonF>& decomposedPolygons)
925 int decompSize = std::numeric_limits<int>::max();
926 if (polygon.size() < 3)
return;
927 if (polygon.size() == 3) {
928 decomposedPolygons << polygon;
932 QList<QPolygonF> decomposedPolygonsMin{};
934 for (
auto vertex = polygon.begin(); vertex != polygon.end(); ++vertex)
937 bool vertexIsReflex = _VertexIsReflex(polygon, vertex);
939 if (!vertexIsReflex)
continue;
941 for (
auto vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther)
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;
953 auto polyLeftContainsReflex =
false;
954 while ( v != vertexOther) {
955 if (v != vertex && _VertexIsReflex(polygon, v)) {
956 polyLeftContainsReflex =
true;
960 if (v == polygon.end()) v = polygon.begin();
962 polyLeft << *vertexOther;
963 auto polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3);
967 auto polyRightContainsReflex =
false;
968 while ( v != vertex) {
969 if (v != vertex && _VertexIsReflex(polygon, v)) {
970 polyRightContainsReflex =
true;
974 if (v == polygon.end()) v = polygon.begin();
976 polyRight << *vertex;
977 auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3);
979 if (!polyLeftValid || ! polyRightValid) {
985 QList<QPolygonF> polyLeftDecomposed{};
986 _PolygonDecomposeConvex(polyLeft, polyLeftDecomposed);
988 QList<QPolygonF> polyRightDecomposed{};
989 _PolygonDecomposeConvex(polyRight, polyRightDecomposed);
992 auto subSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
993 if ((polyLeftContainsReflex && polyLeftDecomposed.size() == 1)
994 || (polyRightContainsReflex && polyRightDecomposed.size() == 1))
997 subSize = std::numeric_limits<int>::max();
999 if (subSize < decompSize) {
1000 decompSize = subSize;
1001 decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
1008 if (decomposedPolygonsMin.size() > 0) {
1009 decomposedPolygons << decomposedPolygonsMin;
1011 decomposedPolygons << polygon;
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;
1025 bool visible =
true;
1027 QLineF lineAB{*vertexA, *vertexB};
1028 auto distanceAB = lineAB.length();
1031 for (
auto vertexC = polygon.begin(); vertexC != polygon.end(); ++vertexC)
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{};
1041 auto intersects = lineAB.intersects(lineCD, &intersection);
1042 if (intersects == QLineF::IntersectType::BoundedIntersection) {
1047 QLineF lineIntersection{*vertexA, intersection};
1048 auto distanceIntersection = lineIntersection.length();
1049 qCDebug(SurveyComplexItemLog) <<
"_VertexCanSeeOther distanceIntersection " << distanceIntersection;
1050 if (distanceIntersection < distanceAB) {
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())));
1070void SurveyComplexItem::_rebuildTransectsFromPolygon(
bool refly,
const QPolygonF& polygon,
const QGeoCoordinate& tangentOrigin,
const QPointF*
const transitionPoint)
1074 double gridAngle = _gridAngleFact.rawValue().toDouble();
1077 gridAngle = _clampGridAngle90(gridAngle);
1078 gridAngle += refly ? 90 : 0;
1079 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 Clamped grid angle" << gridAngle;
1081 qCDebug(SurveyComplexItemLog) <<
"_rebuildTransectsPhase1 gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly;
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();
1093 QList<QLineF> lineList;
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;
1106 lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle));
1107 transectX += gridSpacing;
1111 QList<QLineF> intersectLines;
1113 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
1116 intersectLines = lineList;
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);
1129 lineList.append(firstLine);
1130 intersectLines = lineList;
1131 _intersectLinesWithPolygon(lineList, polygon, intersectLines);
1136 QList<QLineF> resultLines;
1137 _adjustLineDirection(intersectLines, resultLines);
1140 QList<QList<QGeoCoordinate>> transects;
1142 if (transitionPoint !=
nullptr) {
1143 QList<QGeoCoordinate> transect;
1144 QGeoCoordinate coord;
1146 transect.append(coord);
1147 transect.append(coord);
1148 transects.append(transect);
1151 for (
const QLineF& line: resultLines) {
1152 QList<QGeoCoordinate> transect;
1153 QGeoCoordinate coord;
1156 transect.append(coord);
1158 transect.append(coord);
1160 transects.append(transect);
1163 _adjustTransectsToEntryPointLocation(transects);
1166 _optimizeTransectsForShortestDistance(
_transects.last().last().coord, transects);
1169 if (_flyAlternateTransectsFact.rawValue().toBool()) {
1170 QList<QList<QGeoCoordinate>> alternatingTransects;
1171 for (
int i=0; i<transects.count(); i++) {
1173 alternatingTransects.append(transects[i]);
1176 for (
int i=transects.count()-1; i>0; i--) {
1178 alternatingTransects.append(transects[i]);
1181 transects = alternatingTransects;
1185 bool reverseVertices =
false;
1186 for (
int i=0; i<transects.count(); i++) {
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]);
1195 transectVertices = reversedVertices;
1197 reverseVertices =
true;
1199 transects[i] = transectVertices;
1203 for (
const QList<QGeoCoordinate>& transect: transects) {
1204 QGeoCoordinate coord;
1205 QList<TransectStyleComplexItem::CoordInfo_t> coordInfoTransect;
1209 coordInfoTransect.append(coordInfo);
1211 coordInfoTransect.append(coordInfo);
1215 double transectLength = transect[0].distanceTo(transect[1]);
1216 double transectAzimuth = transect[0].azimuthTo(transect[1]);
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);
1229 if (_hasTurnaround()) {
1230 QGeoCoordinate turnaroundCoord;
1233 double azimuth = transect[0].azimuthTo(transect[1]);
1235 turnaroundCoord.setAltitude(qQNaN());
1237 coordInfoTransect.prepend(turnaroundCoordInfo);
1239 azimuth = transect.last().azimuthTo(transect[transect.count() - 2]);
1241 turnaroundCoord.setAltitude(qQNaN());
1243 coordInfoTransect.append(coordInfo);
1248 qCDebug(SurveyComplexItemLog) <<
"_transects.size() " <<
_transects.size();
1251void SurveyComplexItem::_recalcCameraShots(
void)
1255 if (triggerDistance == 0) {
1268 _cameraShots += missionItem->command() == MAV_CMD_IMAGE_START_CAPTURE ? 1 : 0;
1271 bool waitingForTriggerStop =
false;
1272 QGeoCoordinate distanceStartCoord;
1273 QGeoCoordinate distanceEndCoord;
1275 if (missionItem->command() == MAV_CMD_NAV_WAYPOINT) {
1276 if (waitingForTriggerStop) {
1277 distanceEndCoord = QGeoCoordinate(missionItem->param5(), missionItem->param6());
1279 distanceStartCoord = QGeoCoordinate(missionItem->param5(), missionItem->param6());
1281 }
else if (missionItem->command() == MAV_CMD_DO_SET_CAM_TRIGG_DIST) {
1282 if (missionItem->param1() > 0) {
1284 waitingForTriggerStop =
true;
1287 waitingForTriggerStop =
false;
1289 distanceStartCoord = QGeoCoordinate();
1290 distanceEndCoord = QGeoCoordinate();
1298 for (
const QList<TransectStyleComplexItem::CoordInfo_t>& transect:
_transects) {
1299 QGeoCoordinate firstCameraCoord, lastCameraCoord;
1301 firstCameraCoord = transect[1].coord;
1302 lastCameraCoord = transect[transect.count() - 2].coord;
1304 firstCameraCoord = transect.first().coord;
1305 lastCameraCoord = transect.last().coord;
1341 double hoverTime = 0;
1344 for (
const QList<TransectStyleComplexItem::CoordInfo_t>& transect:
_transects) {
1352void SurveyComplexItem::_updateWizardMode(
void)
Geographic coordinate conversion utilities using GeographicLib.
#define QGC_LOGGING_CATEGORY(name, categoryStr)
void _setCameraNameFromV3TransectLoad(const QString &cameraName)
Fact * imageDensity(void)
Fact * adjustedFootprintSide(void)
void setDistanceMode(QGroundControlQmlGlobal::AltMode altMode)
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 * 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 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
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.
double timeBetweenShots(void) final
static constexpr const char * jsonComplexItemTypeValue
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
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
constexpr const char * jsonVersionKey
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)
void convertNedToGeo(double x, double y, double z, const QGeoCoordinate &origin, QGeoCoordinate &coord)