zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Polygon grid support 

Still a bit buggy
QGC4.4
Don Gagne 9 years ago
parent
0ffaea19ec
commit
13fcb41126
2 changed files with 149 additions and 47 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 181
      src/MissionManager/ComplexMissionItem.cc
  2. 15
      src/MissionManager/ComplexMissionItem.h

181
src/MissionManager/ComplexMissionItem.cc
Unescape View File

@ -25,6 +25,8 @@ This file is part of the QGROUNDCONTROL project @@ -25,6 +25,8 @@ This file is part of the QGROUNDCONTROL project
#include "MissionController.h"
#include "QGCGeo.h"
#include <QPolygonF>
const char* ComplexMissionItem::_jsonVersionKey = "version";
const char* ComplexMissionItem::_jsonTypeKey = "type";
const char* ComplexMissionItem::_jsonPolygonKey = "polygon";
@ -252,17 +254,17 @@ void ComplexMissionItem::_generateGrid(void) @@ -252,17 +254,17 @@ void ComplexMissionItem::_generateGrid(void)
_gridPoints.clear();
QList<Point_t> polygonPoints;
QList<Point_t> gridPoints;
QList<QPointF> polygonPoints;
QList<QPointF> gridPoints;
// Convert polygon to NED
// Convert polygon to Qt coordinate system (y positive is down)
qDebug() << "Convert polygon";
QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>();
for (int i=0; i<_polygonPath.count(); i++) {
double x, y, z;
convertGeoToNed(_polygonPath[i].value<QGeoCoordinate>(), tangentOrigin, &x, &y, &z);
polygonPoints += Point_t(x, y);
qDebug() << _polygonPath[i].value<QGeoCoordinate>() << polygonPoints.last().x << polygonPoints.last().y;
double y, x, down;
convertGeoToNed(_polygonPath[i].value<QGeoCoordinate>(), tangentOrigin, &y, &x, &down);
polygonPoints += QPointF(x, -y);
qDebug() << _polygonPath[i].value<QGeoCoordinate>() << polygonPoints.last().x() << polygonPoints.last().y();
}
// Generate grid
@ -270,10 +272,10 @@ void ComplexMissionItem::_generateGrid(void) @@ -270,10 +272,10 @@ void ComplexMissionItem::_generateGrid(void)
// Convert to Geo and set altitude
for (int i=0; i<gridPoints.count(); i++) {
Point_t& point = gridPoints[i];
QPointF& point = gridPoints[i];
QGeoCoordinate geoCoord;
convertNedToGeo(point.x, point.y, 0, tangentOrigin, &geoCoord);
convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord);
_gridPoints += QVariant::fromValue(geoCoord);
}
emit gridPointsChanged();
@ -283,54 +285,161 @@ void ComplexMissionItem::_generateGrid(void) @@ -283,54 +285,161 @@ void ComplexMissionItem::_generateGrid(void)
setCoordinate(_gridPoints.first().value<QGeoCoordinate>());
_setExitCoordinate(_gridPoints.last().value<QGeoCoordinate>());
}
}
QPointF ComplexMissionItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
{
QPointF rotated;
double radians = (M_PI / 180.0) * angle;
rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x());
rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y());
return rotated;
}
void ComplexMissionItem::_gridGenerator(const QList<Point_t>& polygonPoints, QList<Point_t>& gridPoints)
void ComplexMissionItem::_intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines)
{
// FIXME: Hack implementataion
QLineF topLine (boundRect.topLeft(), boundRect.topRight());
QLineF bottomLine (boundRect.bottomLeft(), boundRect.bottomRight());
QLineF leftLine (boundRect.topLeft(), boundRect.bottomLeft());
QLineF rightLine (boundRect.topRight(), boundRect.bottomRight());
gridPoints.clear();
for (int i=0; i<lineList.count(); i++) {
QPointF intersectPoint;
QLineF intersectLine;
const QLineF& line = lineList[i];
int foundCount = 0;
if (line.intersect(topLine, &intersectPoint) == QLineF::BoundedIntersection) {
intersectLine.setP1(intersectPoint);
foundCount++;
}
if (line.intersect(rightLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
if (line.intersect(bottomLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
if (line.intersect(leftLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
// Convert polygon to bounding square
if (foundCount == 2) {
resultLines += intersectLine;
}
}
}
Point_t upperLeft = polygonPoints[0];
Point_t lowerRight = polygonPoints[0];
for (int i=1; i<polygonPoints.count(); i++) {
const Point_t& point = polygonPoints[i];
void ComplexMissionItem::_intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines)
{
for (int i=0; i<lineList.count(); i++) {
int foundCount = 0;
QLineF intersectLine;
const QLineF& line = lineList[i];
for (int j=0; j<polygon.count()-1; j++) {
QPointF intersectPoint;
QLineF polygonLine = QLineF(polygon[j], polygon[j+1]);
if (line.intersect(polygonLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
foundCount++;
intersectLine.setP1(intersectPoint);
} else {
foundCount++;
intersectLine.setP2(intersectPoint);
break;
}
}
}
upperLeft.x = std::min(upperLeft.x, point.x);
upperLeft.y = std::max(upperLeft.y, point.y);
lowerRight.x = std::max(lowerRight.x, point.x);
lowerRight.y = std::min(lowerRight.y, point.y);
if (foundCount == 2) {
resultLines += intersectLine;
}
}
qDebug() << "bounding rect" << upperLeft.x << upperLeft.y << lowerRight.x << lowerRight.y;
}
// Create simplistic set of parallel lines
void ComplexMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& gridPoints)
{
double gridAngle = _gridAngleFact.rawValue().toDouble();
QList<QPair<Point_t, Point_t>> lineList;
gridPoints.clear();
double x = upperLeft.x;
double gridSpacing = _gridSpacingFact.rawValue().toDouble();
while (x < lowerRight.x) {
double yTop = upperLeft.y;
double yBottom = lowerRight.y;
// Convert polygon to bounding rect
lineList += qMakePair(Point_t(x, yTop), Point_t(x, yBottom));
qDebug() << "line" << lineList.last().first.x<< lineList.last().first.y<< lineList.last().second.x<< lineList.last().second.y;
qDebug() << "Polygon";
QPolygonF polygon;
for (int i=0; i<polygonPoints.count(); i++) {
qDebug() << polygonPoints[i];
polygon << polygonPoints[i];
}
polygon << polygonPoints[0];
QRectF smallBoundRect = polygon.boundingRect();
QPointF center = smallBoundRect.center();
qDebug() << "Bounding rect" << smallBoundRect.topLeft().x() << smallBoundRect.topLeft().y() << smallBoundRect.bottomRight().x() << smallBoundRect.bottomRight().y();
// Rotate the bounding rect around it's center to generate the larger bounding rect
QPolygonF boundPolygon;
boundPolygon << _rotatePoint(smallBoundRect.topLeft(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.topRight(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.bottomRight(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.bottomLeft(), center, gridAngle);
boundPolygon << boundPolygon[0];
QRectF largeBoundRect = boundPolygon.boundingRect();
qDebug() << "Rotated bounding rect" << largeBoundRect.topLeft().x() << largeBoundRect.topLeft().y() << largeBoundRect.bottomRight().x() << largeBoundRect.bottomRight().y();
// Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
// bounding box to guarantee intersection.
QList<QLineF> lineList;
float x = largeBoundRect.topLeft().x();
float gridSpacing = _gridSpacingFact.rawValue().toDouble();
while (x < largeBoundRect.bottomRight().x()) {
float yTop = largeBoundRect.topLeft().y() - 100.0;
float yBottom = largeBoundRect.bottomRight().y() + 100.0;
lineList += QLineF(_rotatePoint(QPointF(x, yTop), center, gridAngle), _rotatePoint(QPointF(x, yBottom), center, gridAngle));
qDebug() << "line" << lineList.last().x1() << lineList.last().y1() << lineList.last().x2() << lineList.last().y2();
x += gridSpacing;
}
// Turn into a path
// Now intesect the lines with the smaller bounding rect
QList<QLineF> resultLines;
//_intersectLinesWithRect(lineList, smallBoundRect, resultLines);
_intersectLinesWithPolygon(lineList, polygon, resultLines);
for (int i=0; i<lineList.count(); i++) {
const QPair<Point_t, Point_t> points = lineList[i];
// Turn into a path
for (int i=0; i<resultLines.count(); i++) {
const QLineF& line = resultLines[i];
if (i & 1) {
gridPoints << points.second << points.first;
gridPoints << line.p2() << line.p1();
} else {
gridPoints << points.first << points.second;
gridPoints << line.p1() << line.p2();
}
}
}

15
src/MissionManager/ComplexMissionItem.h
Unescape View File

@ -95,21 +95,14 @@ signals: @@ -95,21 +95,14 @@ signals:
void gridPointsChanged(void);
private:
typedef struct Point_s {
double x;
double y;
Point_s(double x, double y) {
this->x = x;
this->y = y;
}
} Point_t;
void _clear(void);
void _setExitCoordinate(const QGeoCoordinate& coordinate);
void _clearGrid(void);
void _generateGrid(void);
void _gridGenerator(const QList<Point_t>& polygonPoints, QList<Point_t>& gridPoints);
void _gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& gridPoints);
QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
void _intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines);
void _intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines);
int _sequenceNumber;
bool _dirty;

Write Preview
Loading…
Cancel
Save