diff --git a/deploy/QGroundControl.AppImage b/deploy/QGroundControl.AppImage index c841f9730473d586da211149df65ed8019aaf710..9b667e360c552b058fe23d07f4d14784ee43c1d9 100755 Binary files a/deploy/QGroundControl.AppImage and b/deploy/QGroundControl.AppImage differ diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index aa5ee34d48e50cd886969dd722b36b9ab984c642..0c776075649a82d5e4e4477dc7a6cb950958943d 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -75,21 +75,20 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() QVector distances; for (const QPointF &p : surveyPolygon) distances.append(norm(p)); - - double dalpha = 0.5/180*M_PI; // radiants - double dr = 30; // meter + double dalpha = 2.1/180*M_PI; // radiants + double dr = 3; // meter double r_min = dr; // meter double r_max = (*std::max_element(distances.begin(), distances.end())); // meter - QPointF origin(0, 0); IntersectType type; + bool originInside = true; if (!contains(surveyPolygon, origin, type)) { QVector angles; for (const QPointF &p : surveyPolygon) angles.append(truncateAngle(angle(p))); // determine r_min by successive approximation - double r = r_max; + double r = r_max - dr; while ( r > r_min) { Circle circle(r, origin); @@ -100,6 +99,7 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() r -= dr; } + originInside = false; } // qWarning("r_min, r_max:"); @@ -110,82 +110,124 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() decomposeToConvex(surveyPolygon, convexPolygons); QList> fullPath; - for (int i = 0; i < convexPolygons.size(); i++){ + for (int i = 0; i < convexPolygons.size(); i++) { const QPolygonF &polygon = convexPolygons[i]; double r = r_min; QList> currPolyPath; - bool currentPolyPathUpdated = false; while (r < r_max) { Circle circle(r, origin); QList intersectPoints; QList typeList; QList> neighbourList; if (intersects(circle, polygon, intersectPoints, neighbourList, typeList)) { - if (intersectPoints.size() >= 1) { - - for (int i = 0; i < intersectPoints.size(); i++) { - QList intersects = intersectPoints[i]; - - QPointF p1 = polygon[neighbourList[i].first]; - QPointF p2 = polygon[neighbourList[i].second]; - QLineF intersetLine(p1, p2); - double lineAngle = truncateAngle(angle(intersetLine)); - - double alpha1 = 0; - bool skip = true; - QPointF vertex; - for (QPointF ipt : intersects) { - double circleTangentAngle = truncateAngle(angle(ipt)+M_PI_2); - if (lineAngle > circleTangentAngle && lineAngle < circleTangentAngle + M_PI) { - alpha1 = truncateAngle(circleTangentAngle-M_PI_2); - vertex = ipt; - skip = false; + + // intersection Points between circle and polygon, entering polygon + // when walking in counterclockwise direction along circle + QPointFList entryPoints; + // intersection Points between circle and polygon, leaving polygon + // when walking in counterclockwise direction along circle + QPointFList exitPoints; + // determine entryPoints and exit Points + for (int j = 0; j < intersectPoints.size(); j++) { + QList intersects = intersectPoints[j]; + + QPointF p1 = polygon[neighbourList[j].first]; + QPointF p2 = polygon[neighbourList[j].second]; + QLineF intersetLine(p1, p2); + double lineAngle = truncateAngle(angle(intersetLine)); + + for (QPointF ipt : intersects) { + double circleTangentAngle = truncateAngle(angle(ipt)+M_PI_2); + // compare line angle and circle tangent at intersection point + // to determine between exit and entry point + if ( !qFuzzyCompare(lineAngle, circleTangentAngle) + && !qFuzzyCompare(lineAngle, truncateAngle(circleTangentAngle + M_PI))) { + if (truncateAngle(lineAngle - circleTangentAngle) < M_PI) { + entryPoints.append(ipt); + } else { + exitPoints.append(ipt); } } + } + } + // sort + std::sort(entryPoints.begin(), entryPoints.end(), [](QPointF p1, QPointF p2) { + return angle(p1) < angle(p2); + }); + std::sort(exitPoints.begin(), exitPoints.end(), [](QPointF p1, QPointF p2) { + return angle(p1) < angle(p2); + }); + + // match entry and exit points + int offset = 0; + double minAngle = std::numeric_limits::infinity(); + for (int k = 0; k < exitPoints.size(); k++) { + QPointF pt = exitPoints[k]; + double alpha = truncateAngle(angle(pt) - angle(entryPoints[0])); + if (minAngle > alpha) { + minAngle = alpha; + offset = k; + } + } - QList sectorPath; - // walk clockwisely along the circle, and break when leaving the polygon - if (!skip) { - double alpha = alpha1; - do { - sectorPath.append(vertex); - alpha += dalpha; - circle.toCoordinate(vertex, alpha); - } while (polygon.containsPoint(vertex, Qt::FillRule::OddEvenFill)); - } + for (int k = 0; k < entryPoints.size(); k++) { + double alpha1 = angle(entryPoints[k]); + double alpha2 = angle(exitPoints[(k+offset) % entryPoints.size()]); - // use shortestPath() here if necessary, could be a problem if dr >> + QList sectorPath = circle.approximateSektor(dalpha, alpha1, alpha2); + // use shortestPath() here if necessary, could be a problem if dr >> + if (sectorPath.size() > 0) currPolyPath.append(sectorPath); - currentPolyPathUpdated = true; - } - } + } else if (originInside) { + // circle fully inside polygon + QList sectorPath = circle.approximateSektor(dalpha, 0, 2*M_PI); + // use shortestPath() here if necessary, could be a problem if dr >> + currPolyPath.append(sectorPath); } r += dr; + } + if (currPolyPath.size() > 0) { + fullPath.append(currPolyPath); } + } - - - if (currentPolyPathUpdated) { - if (fullPath.size() > 1) { - QPointF start = fullPath.last().last(); - QPointF end = currPolyPath.last().first(); - QList path; - if(!shortestPath(surveyPolygon, start, end, path)) - return; - path.removeFirst(); - path.removeLast(); - currPolyPath.last().append(path); + // optimize path to lawn pattern + QList currentSection = fullPath.takeFirst(); + QList> optiPath; // optimized path + while( !fullPath.empty() ) { + optiPath.append(currentSection); + QPointF endVertex = currentSection.last(); + double minDist = std::numeric_limits::infinity(); + int index = 0; + bool reversePath = false; + + // iterate over all paths in fullPath and assign the one with the shortest distance to endVertex to currentSection + for (int i = 0; i < fullPath.size(); i++) { + auto iteratorPath = fullPath[i]; + double dist = PlanimetryCalculus::distance(endVertex, iteratorPath.first()); + if ( dist < minDist ) { + minDist = dist; + index = i; + } + dist = PlanimetryCalculus::distance(endVertex, iteratorPath.last()); + if (dist < minDist) { + minDist = dist; + index = i; + reversePath = true; } - - fullPath.append(currPolyPath); } - + currentSection = fullPath.takeAt(index); + if (reversePath) { + PolygonCalculus::reversePath(currentSection); + } } - for ( const QList &transect : fullPath) { + + // convert to CoordInfo_t + for ( const QList &transect : optiPath) { QList geoPath = toGeo(transect, _referencePoint); QList transectList; for ( const QGeoCoordinate &coordinate : geoPath) { diff --git a/src/Wima/PolygonCalculus.cc b/src/Wima/PolygonCalculus.cc index daecc6689694aeb5e96f754286e4c6f3ce24a9e7..871165a747d92856d29e7e5774e769361ce47f70 100644 --- a/src/Wima/PolygonCalculus.cc +++ b/src/Wima/PolygonCalculus.cc @@ -482,12 +482,12 @@ namespace PolygonCalculus { bool shortestPath(QPolygonF polygon, QPointF startVertex, const QPointF &endVertex, QList &shortestPath) { using namespace PlanimetryCalculus; - offsetPolygon(polygon, 0.01); + offsetPolygon(polygon, 0.01); // solves numerical errors if ( contains(polygon, startVertex) && contains(polygon, endVertex)) { // lambda QPolygonF polygon2 = polygon; - offsetPolygon(polygon2, 0.01); + offsetPolygon(polygon2, 0.01); // solves numerical errors std::function distance = [polygon2](const QPointF &p1, const QPointF &p2) -> double { if (containsPath(polygon2, p1, p2)){ double dx = p1.x()-p2.x();