Skip to content

Q
qgroundcontrol
Commit 426293a7 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

debugging, containsPoint in dijkstra not working

parent 8d84693d
Hide whitespace changes
Inline Side-by-side
Showing with 280 additions and 510 deletions
qgroundcontrol.pro
View file @ 426293a7
......@@ -457,9 +457,9 @@ SOURCES += \
src/Wima/CircularSurveyComplexItem.cc \
src/Wima/PlanimetryCalculus.cc \
src/Wima/Circle.cc \
src/Wima/PolygonCalculus.cc \
src/Wima/OptimisationTools.cc \
src/Wima/GeoUtilities.cc
src/Wima/GeoUtilities.cc \
src/Wima/PolygonCalculus.cc
#
# Unit Test specific configuration goes here (requires full debug build with all plugins)
......
src/Wima/CircularSurveyComplexItem.cc
View file @ 426293a7
......@@ -11,12 +11,12 @@ CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyV
bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString)
{
return TransectStyleComplexItem::load(complexObject, sequenceNumber, errorString);
return false;
}
void CircularSurveyComplexItem::save(QJsonArray &planItems)
{
TransectStyleComplexItem::save(planItems);
}
void CircularSurveyComplexItem::appendMissionItems(QList<MissionItem *> &items, QObject *missionItemParent)
......
src/Wima/GeoUtilities.cc
View file @ 426293a7
#include "GeoUtilities.h"
#include <QtMath>
QGeoCoordinate GeoUtilites::toGeo(const QVector3D &point, const QGeoCoordinate &origin)
{
double x = point.x();
double y = point.y();
double z = point.z();
namespace GeoUtilities {
double lat = origin.latitude()/180*M_PI;
double lon = origin.longitude()/180*M_PI;
double h = origin.altitude();
QGeoCoordinate toGeo(const QVector3D &point, const QGeoCoordinate &origin)
{
using namespace PolygonCalculus;
double z = point.z();
double h = origin.altitude();
if (!qFuzzyCompare(lat, M_PI_2)) // this could (unlikely) be a problem, replace with different coordinate transformation
return QGeoCoordinate(/* lat */ qAtan(x/cos(lat)/earthRadius) + lon,
/* lon */ qAtan(y/earthRadius) + lat,
/* alt */ h + z);
else
return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf)
}
QGeoCoordinate coordinate(toGeo(point.toPointF(), origin));
coordinate.setAltitude(z + h);
return coordinate;
QVector3D GeoUtilites::toCartesian(const QGeoCoordinate &point, const QGeoCoordinate &origin)
{
double lat = point.latitude()/180*M_PI;
double lon = point.longitude()/180*M_PI;
double h = point.altitude();
}
double latO = origin.latitude()/180*M_PI;
double lonO = origin.longitude()/180*M_PI;
double hO = origin.altitude();
QVector3D toCartesian(const QGeoCoordinate &coordinate, const QGeoCoordinate &origin)
{
double dlon = lon-lonO;
double dlat = lat-latO;
double h = coordinate.altitude();
double hO = origin.altitude();
QVector3D point(toCartesian2D(coordinate, origin));
point.setZ(h - hO);
if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2))
return QVector3D(/* x */ qTan(dlon)*earthRadius*qCos(latO),
/* y */ qTan(dlat)*earthRadius,
/* z */ h - hO);
else
return QVector3D(); // singularity occurred (tan(pi/2) = inf)
return point;
}
}
QGeoList toGeo(const QVector3DList &points, const QGeoCoordinate &origin)
{
QGeoList coordinates;
for (auto point : points)
coordinates.append(toGeo(point, origin));
GeoUtilites::QGeoList GeoUtilites::toGeo(const GeoUtilites::QVector3DFList &points, const QGeoCoordinate &origin)
{
GeoUtilites::QGeoList coordinates;
for (auto point : points)
coordinates.append(toGeo(point, origin));
return coordinates;
}
return coordinates;
}
QVector3DList toCartesian(const QGeoList &coordinates, const QGeoCoordinate &origin)
{
QVector3DList points;
for (auto coordinate : coordinates )
points.append(toCartesian(coordinate, origin));
return points;
}
QPointF toCartesian2D(const QGeoCoordinate &point, const QGeoCoordinate &origin)
{
double lat = point.latitude()/180*M_PI;
double lon = point.longitude()/180*M_PI;
double latO = origin.latitude()/180*M_PI;
double lonO = origin.longitude()/180*M_PI;
double dlon = lon-lonO;
double dlat = lat-latO;
if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2))
return QPointF(/* x */ qTan(dlon)*earthRadius*qCos(latO),
/* y */ qTan(dlat)*earthRadius);
else
return QPointF(); // singularity occurred (tan(pi/2) = inf)
}
GeoUtilites::QVector3DFList GeoUtilites::toCartesian(const GeoUtilites::QGeoList &coordinates, const QGeoCoordinate &origin)
{
GeoUtilites::QVector3DFList points;
for (auto coordinate : coordinates )
points.append(toCartesian(coordinate, origin));
QPointFList toCartesian2D(const QGeoList &coordinates, const QGeoCoordinate &origin)
{
QPointFList listF;
for ( auto coordinate : coordinates)
listF.append(toCartesian2D(coordinate, origin));
return listF;
}
QGeoCoordinate toGeo(const QPointF &point, const QGeoCoordinate &origin)
{
double x = point.x();
double y = point.y();
double lat = origin.latitude();
double lon = origin.longitude();
//qWarning("%lf %lf %lf %lf", x, y, lat, lon);
if (!qFuzzyCompare(lat, M_PI_2)) // this could (unlikely) be a problem, replace with different coordinate transformation
return QGeoCoordinate(/* lat */ qAtan(y/earthRadius)*180/M_PI + lat,
/* lon */ qAtan(x/cos(lat/180*M_PI)/earthRadius)*180/M_PI + lon,
/* alt */ origin.altitude());
else
return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf)
}
QGeoList toGeo(const QPointFList &points, const QGeoCoordinate &origin)
{
QGeoList coordinates;
for ( auto point : points)
coordinates.append(toGeo(point, origin));
return coordinates;
}
return points;
}
src/Wima/GeoUtilities.h
View file @ 426293a7
#ifndef GEOPOLYGONUTILITIES_H
#define GEOPOLYGONUTILITIES_H
#pragma once
#include <QPointF>
#include <QGeoCoordinate>
#include <QVector3D>
#include <QGeoCoordinate>
#include <QtMath>
#include "PolygonCalculus.h"
namespace GeoUtilites {
typedef QList<QVector3D> QVector3DFList;
namespace GeoUtilities {
typedef QList<QVector3D> QVector3DList;
typedef QList<QPointF> QPointFList;
typedef QList<QGeoCoordinate> QGeoList;
const double earthRadius = 6378137; // meter
QGeoCoordinate toGeo (const QVector3D &point, const QGeoCoordinate &origin);
QGeoList toGeo (const QVector3DFList &points, const QGeoCoordinate &origin);
QVector3D toCartesian (const QGeoCoordinate &point, const QGeoCoordinate &origin);
QVector3DFList toCartesian (const QGeoList &coordinates, const QGeoCoordinate &origin);
QGeoCoordinate toGeo (const QVector3D &point, const QGeoCoordinate &origin);
QGeoList toGeo (const QVector3DList &points, const QGeoCoordinate &origin);
QGeoCoordinate toGeo (const QPointF &point, const QGeoCoordinate &origin);
QGeoList toGeo (const QPointFList &points, const QGeoCoordinate &origin);
QVector3D toCartesian (const QGeoCoordinate &coordinate, const QGeoCoordinate &origin);
QVector3DList toCartesian (const QGeoList &coordinates, const QGeoCoordinate &origin);
QPointF toCartesian2D (const QGeoCoordinate &point, const QGeoCoordinate &origin);
QPointFList toCartesian2D (const QGeoList &coordinates, const QGeoCoordinate &origin);
}
#endif // GEOPOLYGONUTILITIES_H
src/Wima/OptimisationTools.cc
View file @ 426293a7
#include "OptimisationTools.h"
#include <QPointF>
namespace OptimisationTools {
namespace {
......@@ -15,15 +16,15 @@ namespace OptimisationTools {
*
* \sa QList
*/
template <typename T>
bool dijkstraAlgorithm(const QList<T> elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance) // don't seperate parameters with new lines or documentation will break
typedef QPointF T;
bool dijkstraAlgorithm(const QList<T> &elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance) // don't seperate parameters with new lines or documentation will break
{
if ( elements.isEmpty() || startIndex < 0
|| startIndex >= elements.size() || endIndex < 0
|| endIndex >= elements.size()) {
return false;
}
qWarning("optitools");
// Each element of type T gets stuff into a Node
/// @param distance is the distance between the Node and it's predecessor
struct Node{
......
src/Wima/OptimisationTools.h
View file @ 426293a7
#ifndef OPTIMISATIONTOOLS_H
#define OPTIMISATIONTOOLS_H
#pragma once
#include <QObject>
#include <functional>
#include <QPointF>
namespace OptimisationTools {
template <typename T>
bool dijkstraAlgorithm(const QList<T> elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance);
typedef QPointF T;
bool dijkstraAlgorithm(const QList<T> &elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance);
}
#endif // OPTIMISATIONTOOLS_H
src/Wima/PlanimetryCalculus.cc
View file @ 426293a7
......@@ -12,14 +12,14 @@ namespace PlanimetryCalculus {
\sa QPointF, Circle
*/
bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints, IntersectType type, bool calcInstersect)
bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints, IntersectType &type, bool calcInstersect)
{
if (!circle.isNull() && ! line.isNull()) {
QPointF translationVector = line.p1();
double angleWLDegree = line.angle(); // angle between wold and line coordinate system
double alpha = angle(line); // angle between wold and line coordinate system
QPointF originCircleL = circle.origin() - translationVector;
rotateReference(originCircleL, -angleWLDegree); // circle origin in line corrdinate system
rotateReference(originCircleL, -alpha); // circle origin in line corrdinate system
double y = originCircleL.y();
double r = circle.radius();
......@@ -33,7 +33,7 @@ namespace PlanimetryCalculus {
if (x_ori >= 0 && x_ori <= line.length()) {
if (calcInstersect) {
QPointF intersectionPt = QPointF(x_ori, 0);
rotateReference(intersectionPt, angleWLDegree);
rotateReference(intersectionPt, alpha);
intersectionPoints.append(intersectionPt + translationVector);
}
......@@ -54,7 +54,7 @@ namespace PlanimetryCalculus {
if (x1 >= 0 && x1 <= line.length()) { // check if intersection point is on the line
if (calcInstersect) {
QPointF intersectionPt = QPointF(x1, 0); // first intersection point (line system)
rotateReference(intersectionPt, angleWLDegree);
rotateReference(intersectionPt, alpha);
intersectionPoints.append(intersectionPt + translationVector); // transform (to world system) and append first intersection point
}
doesIntersect = true;
......@@ -62,7 +62,7 @@ namespace PlanimetryCalculus {
if (x2 >= 0 && x2 <= line.length()) { // check if intersection point is on the line
if (calcInstersect) {
QPointF intersectionPt = QPointF(x2, 0); // second intersection point (line system)
rotateReference(intersectionPt, angleWLDegree);
rotateReference(intersectionPt, alpha);
intersectionPoints.append(intersectionPt + translationVector); // transform (to world system) and append second intersection point
}
doesIntersect = true;
......@@ -97,7 +97,7 @@ namespace PlanimetryCalculus {
double R = 0;
if (r1 > r2) {
R = r1; // large
r = r2; // small
r = r2; // smallline1
} else {
// this branch is also choosen if r1 == r2
R = r2;
......@@ -256,7 +256,7 @@ namespace PlanimetryCalculus {
\fn double distance(const QPointF &p1, const QPointF p2)
Calculates the angle (in degrees) between the line defined by \a p1 and \a p2 and the x-axis according to the following rule.
Angle = qAtan2(dy, dx)*180/pi, where dx = p2.x()-p1.x() and dy = p2.y()-p1.y().
angle
\note The order of \a p1 and \a p2 matters. Swapping \a p1 and \a p2 will result in a angle of oposite sign.
\sa QPointF
*/
......@@ -307,10 +307,10 @@ namespace PlanimetryCalculus {
// line 1 coordinate system: origin line1.p1(), x-axis towards line1.p2()
QPointF translationVector = line1.p1(); // translation vector between world and line1 system
double alpha = line1.angle();
double alpha = angle(line1);
double l1 = line1.length();
QLineF line2L1 = line1;
QLineF line2L1 = line2;
line2L1.translate(-translationVector);
rotateReference(line2L1, -alpha);
......@@ -350,7 +350,7 @@ namespace PlanimetryCalculus {
}
if (xNull >= x1 && xNull <= x2){
// determine intersection type
// determine intersection type#include "QVector3D"
if(qFuzzyIsNull(xNull) || qFuzzyCompare(xNull, l1)) {
if(qFuzzyIsNull(y1) || qFuzzyIsNull(y2))
......@@ -397,7 +397,6 @@ namespace PlanimetryCalculus {
{
if (polygon.size() >= 2) {
neighbourList.clear();
IntersectList intersectionTypeList;
// Assemble a line form each tow consecutive polygon vertices and check whether it intersects with line
for (int i = 0; i < polygon.size(); i++) {
......@@ -542,6 +541,12 @@ namespace PlanimetryCalculus {
return intersects(circle1, circle2, intersectionPoints, type, true /*calculate intersection points*/);
}
;
double angle(const QLineF &line)
{
return angle(line.p1(), line.p2());
}
......
src/Wima/PlanimetryCalculus.h
View file @ 426293a7
......@@ -57,6 +57,7 @@ namespace PlanimetryCalculus {
double distance(const QPointF &p1, const QPointF p2);
double angle(const QPointF &p1, const QPointF p2);
double angle(const QLineF &line);
double angleDegree(const QPointF &p1, const QPointF p2);
double truncateAngle(double angle);
double truncateAngleDegree(double angle);
......
src/Wima/PolygonCalculus.cc
View file @ 426293a7
#include "PolygonCalculus.h"
#include "PlanimetryCalculus.h"
#include "OptimisationTools.h"
#include <QVector3D>
#include <functional>
namespace PolygonCalculus {
namespace {
......@@ -75,7 +81,7 @@ namespace PolygonCalculus {
}
}
/*!
/*!auto distance
* \fn QPointF closestVertex(const QPolygonF &polygon, const QPointF &coordinate);
* Returns the vertex of \a polygon with the least distance to \a coordinate.
*
......@@ -137,8 +143,9 @@ namespace PolygonCalculus {
* or one area is inside the other.
* The algorithm assumes that \a joinedPolygon is empty.
*/
JoinPolygonError joinPolygon(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon)
JoinPolygonError join(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon)
{
using namespace PolygonCalculus;
if (polygon1.size() >= 3 && polygon2.size() >= 3) {
if ( !isSimplePolygon(polygon1) || !isSimplePolygon(polygon2)) {
......@@ -193,7 +200,7 @@ namespace PolygonCalculus {
//qDebug("IntersectionList.size(): %i", intersectionList.size());
if (intersectionList.size() >= 1) { bool containsPath (const QPointF &c1, const QPointF &c2, QPolygonF polygon);
if (intersectionList.size() >= 1) {
int minDistIndex = 0;
// find the vertex with the least distance to currentVertex
......@@ -305,7 +312,7 @@ namespace PolygonCalculus {
/*!
* \fn bool hasClockwiseWinding(const QPolygonF &polygon)
* Returns \c true if \a path has clockwise winding, \c false else.
* Returns \c true if \a path has clockwiauto distancese winding, \c false else.
*/
bool hasClockwiseWinding(const QPolygonF &polygon)
{
......@@ -351,7 +358,7 @@ namespace PolygonCalculus {
QPolygonF newPolygon;
if (polygon.size() > 2) {
// Walk the edges, offsetting by the specified distance
// Walk the edges, offsetting by theauto distance specified distance
QList<QLineF> rgOffsetEdges;
for (int i = 0; i < polygon.size(); i++) {
int nextIndex = nextVertexIndex(polygon.size(), i);
......@@ -469,7 +476,6 @@ namespace PolygonCalculus {
decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
}
}
}
// assemble output
......@@ -487,7 +493,7 @@ namespace PolygonCalculus {
if ( polygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill)
&& polygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) {
// lambda
auto distance = [polygon](const QPointF &p1, const QPointF &p2) -> double {
std::function<double(const QPointF &, const QPointF &)> distance = [polygon](const QPointF &p1, const QPointF &p2) -> double {
if (containsPath(polygon, p1, p2)){
double dx = p1.x()-p2.x();
double dy = p1.y()-p2.y();
......@@ -502,13 +508,40 @@ namespace PolygonCalculus {
for (int i = 0; i < polygon.size(); i++) {
elementList.append(polygon[i]);
}
return OptimisationTools::dijkstraAlgorithm<QPointF>(elementList, 0, 1, shortestPath, distance);
qWarning("Hi");
return OptimisationTools::dijkstraAlgorithm(elementList, 0, 1, shortestPath, distance);
} else {
return false;
}
}
QVector3DList toQVector3DList(const QPolygonF &polygon)
{
QVector3DList list;
for ( auto vertex : polygon )
list.append(QVector3D(vertex));
return list;
}
QPolygonF toQPolygonF(const QPointFList &listF)
{
QPolygonF polygon;
for ( auto vertex : listF )
polygon.append(vertex);
return polygon;
}
QPointFList toQPointFList(const QPolygonF &polygon)
{
QPointFList listF;
for ( auto vertex : polygon )
listF.append(vertex);
return listF;
}
} // end PolygonCalculus namespace
src/Wima/PolygonCalculus.h
View file @ 426293a7
#ifndef POLYGONCALCULUS_H
#define POLYGONCALCULUS_H
#endif
#pragma once
#include <QPointF>
#include <QPolygonF>
#include <QVector3D>
#include "PlanimetryCalculus.h"
#include "OptimisationTools.h"
namespace PolygonCalculus {
enum JoinPolygonError { NotSimplePolygon, PolygonJoined, Disjoint, PathSizeLow};
typedef QList<QVector3D> QVector3DList;
typedef QList<QPointF> QPointFList;
int closestVertexIndex (const QPolygonF &polygon, const QPointF &coordinate);
QPointF closestVertex (const QPolygonF &polygon, const QPointF &coordinate);
int nextVertexIndex (int pathsize, int index);
int previousVertexIndex (int pathsize, int index);
JoinPolygonError joinPolygon (QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon);
JoinPolygonError join (QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon);
bool isSimplePolygon (const QPolygonF &polygon);
bool hasClockwiseWinding (const QPolygonF &path);
void reversePath (QPolygonF &path);
......@@ -25,6 +26,14 @@ namespace PolygonCalculus {
bool containsPath (QPolygonF polygon, const QPointF &c1, const QPointF &c2);
void decomposeToConvex (const QPolygonF &polygon, QList<QPolygon> &convexPolygons);
bool shortestPath (const QPolygonF &polygon, const QPointF &startVertex, const QPointF &endVertex, QList<QPointF> &shortestPath);
QPolygonF toQPolygonF(const QVector3DList &polygon);
QPolygonF toQPolygonF(const QPointFList &polygon);
QLineF toQLineF(const QVector3DList &line);
QPointFList toQPointFList(const QVector3DList &list);
QPointFList toQPointFList(const QPolygonF &list);
QVector3DList toQVector3DList(const QPointFList &listF);
QVector3DList toQVector3DList(const QPolygonF &listF);
}
src/Wima/WimaArea.cc
View file @ 426293a7
#include "WimaArea.h"
#include "WimaArea.h"
/*!
* \variable WimaArea::epsilonMeter
......@@ -138,122 +139,55 @@ QGCMapPolygon WimaArea::toQGCPolygon() const
* The algorithm will be able to join the areas, if either their edges intersect with each other,
* or one area contains the other.
*/
bool WimaArea::join(WimaArea &area1, WimaArea &area2, WimaArea &joinedArea, QString &errorString)
bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &joinedArea, QString &errorString)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
if (area1.count() >= 3 && area2.count() >= 3) {
QList<QGeoCoordinate> GeoPolygon1 = area1.coordinateList();
QList<QGeoCoordinate> GeoPolygon2 = area2.coordinateList();
if ( isSelfIntersecting(area1) ) {
errorString.append("Area 1 is self intersecting.\n");
return false;
}
/*qWarning("befor joining");
qWarning() << GeoPolygon1;
qWarning() << GeoPolygon2;*/
if ( isSelfIntersecting(area2) ) {
errorString.append("Area 2 is self intersecting.\n");
return false;
}
QGeoCoordinate origin = GeoPolygon1[0];
joinedArea.clear();
area1.verifyClockwiseWinding();
area2.verifyClockwiseWinding();
WimaArea* walkerPoly = &area1; // "walk" on this polygon towards higher indices
WimaArea* crossPoly = &area2; // check for crossings with this polygon while "walking"
// and swicht to this polygon on a intersection,
// continue to walk towards higher indices
// begin with the first index which is not inside crosspoly, if all Vertices are inside crosspoly return crosspoly
int startIndex = 0;
bool crossContainsWalker = true;
for (int i = 0; i < walkerPoly->count(); i++) {
if ( !crossPoly->containsCoordinate(walkerPoly->vertexCoordinate(i)) ) {
crossContainsWalker = false;
startIndex = i;
break;
}
}
QGeoCoordinate tset = GeoPolygon1[2];
//qWarning() << tset;qWarning() << toGeo(toCartesian2D(tset, origin), origin);
if ( crossContainsWalker == true) {
joinedArea.appendVertices(crossPoly->coordinateList());
return true;
}
QGeoCoordinate currentVertex = walkerPoly->vertexCoordinate(startIndex);
QGeoCoordinate startVertex = currentVertex;
// possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly)
QGeoCoordinate protoNextVertex = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(startIndex));
int nextVertexIndex = walkerPoly->nextVertexIndex(startIndex);
while (1) {
//qDebug("nextVertexIndex: %i", nextVertexIndex);
joinedArea.appendVertex(currentVertex);
QGCMapPolyline walkerPolySegment;
walkerPolySegment.appendVertex(currentVertex);
walkerPolySegment.appendVertex(protoNextVertex);
QList<QPair<int, int>> neighbourList;
QList<QGeoCoordinate> intersectionList;
//qDebug("IntersectionList.size() on init: %i", intersectionList.size());
intersects(walkerPolySegment, *crossPoly, intersectionList, neighbourList);
//qDebug("IntersectionList.size(): %i", intersectionList.size());
if (intersectionList.size() >= 1) {
int minDistIndex = 0;
if (intersectionList.size() > 1) {
double minDist = currentVertex.distanceTo(intersectionList.value(minDistIndex));
for (int i = 1; i < intersectionList.size(); i++) {
double currentDist = currentVertex.distanceTo(intersectionList.value(i));
if ( minDist > currentDist ) {
minDist = currentDist;
minDistIndex = i;
}
}
}
//qDebug("MinDistIndex: %i", minDistIndex);
QGeoCoordinate protoCurrentVertex = intersectionList.value(minDistIndex);
// take numerical erros into account
if (protoCurrentVertex.distanceTo(currentVertex) > epsilonMeter) {
currentVertex = protoNextVertex;
QPair<int, int> neighbours = neighbourList.value(minDistIndex);
protoNextVertex = crossPoly->vertexCoordinate(neighbours.second);
nextVertexIndex = neighbours.second;
// swap walker and cross poly
WimaArea* temp = walkerPoly;
walkerPoly = crossPoly;
crossPoly = temp;
} else {
currentVertex = walkerPoly->vertexCoordinate(nextVertexIndex);
protoNextVertex = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(nextVertexIndex));
nextVertexIndex = walkerPoly->nextVertexIndex(nextVertexIndex);
}
} else {
currentVertex = walkerPoly->vertexCoordinate(nextVertexIndex);
protoNextVertex = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(nextVertexIndex));
nextVertexIndex = walkerPoly->nextVertexIndex(nextVertexIndex);
}
if (currentVertex == startVertex) {
if (area1.count() == joinedArea.count()) { // is the case if poly1 and poly2 don't intersect
return false;
} else {
return true;
}
}
}
QPolygonF polygon1 = toQPolygonF(toCartesian2D(GeoPolygon1, origin));
QPolygonF polygon2 = toQPolygonF(toCartesian2D(GeoPolygon2, origin));
} else {
return false;
}
/*qWarning("after 1 transform");
qWarning() << polygon1;
qWarning() << polygon2;*/
QPolygonF joinedPolygon;
JoinPolygonError retValue = PolygonCalculus::join(polygon1, polygon2, joinedPolygon);
/*qWarning("after joining");
qWarning() << joinedPolygon;*/
if (retValue == JoinPolygonError::Disjoint) {
qWarning("Polygons are disjoint.");
} else if (retValue == JoinPolygonError::NotSimplePolygon) {
qWarning("Not a simple polygon.");
} else if (retValue == JoinPolygonError::PathSizeLow) {
qWarning("Polygon vertex count is low.");
} else {
QList<QGeoCoordinate> path = toGeo(toQPointFList(joinedPolygon), origin);
//qWarning("after transform");
//qWarning() << path;
joinedArea.setPath(path);
return true;
}
return false;
}
......@@ -349,298 +283,22 @@ int WimaArea::previousVertexIndex(int index) const
}
}
/*!
* \fn bool WimaArea::intersects(const QGCMapPolyline &line1, const QGCMapPolyline &line2, QGeoCoordinate &intersectionPt)
* Returns \c true if \a line1 and \a line2 intersect with each other.
* Stores the intersection point in \a intersectionPt
*
* \sa QGeoCoordinate
*/
bool WimaArea::intersects(const QGCMapPolyline &line1, const QGCMapPolyline &line2, QGeoCoordinate &intersectionPt)
{
if (line1.count() == 2 && line2.count() == 2 ) {
QPointF pt11(0, 0);
double x, y, z;
QGeoCoordinate origin = line1.vertexCoordinate(0);
convertGeoToNed(line1.vertexCoordinate(1), origin, &x, &y, &z);
QPointF pt12(x, y);
QLineF kartLine1(pt11, pt12);
convertGeoToNed(line2.vertexCoordinate(0), origin, &x, &y, &z);
QPointF pt21(x, y);
convertGeoToNed(line2.vertexCoordinate(1), origin, &x, &y, &z);
QPointF pt22(x, y);;
QLineF kartLine2(pt21, pt22);
QPointF intersectionPoint;
if (kartLine1.intersect(kartLine2, &intersectionPoint) == QLineF::BoundedIntersection) {
convertNedToGeo(intersectionPoint.x(), intersectionPoint.y(), origin.altitude(), origin, &intersectionPt);
return true;
}
else {
return false;
}
} else {
qWarning("WimaArea::intersect(line1, line2): line1->count() != 2 || line2->count() != 2!");
return false;
}
}
/*!
* \fn bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &neighbourList)
* Returns \c true if \a line and \a area intersect with each other at least once.bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &neighbourList)
* Stores the intersection points in \a intersectionList.
* Stores the indices of the closest two \a area vetices for each of coorespoinding intersection points in \a neighbourList.
*
* For example if an intersection point is found between the first and the second vertex of the \a area the intersection point will
* be stored in \a intersectionList and the indices 1 and 2 will be stored in \a neighbourList.
* \a neighbourList has entries of type \c {QPair<int, int>}, where \c{pair.first} would contain 1 and \c{pair.second} would contain 2, when
* relating to the above example.
*
* \sa QPair, QList
*/
bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &neighbourList)// don't seperate parameters with new lines or documentation will break
{
intersectionList.clear();
neighbourList.clear();
if (line.count() == 2 && area.count() >= 3) { // are line a proper line and poly a proper poly?other,
// Asseble a line form each tow consecutive polygon vertices and check whether it intersects with line
for (int i = 0; i < area.count(); i++) {
QGCMapPolyline interatorLine;
QGeoCoordinate currentVertex = area.vertexCoordinate(i);
QGeoCoordinate nextVertex = area.vertexCoordinate(area.nextVertexIndex(i));
interatorLine.appendVertex(currentVertex);
interatorLine.appendVertex(nextVertex);
QGeoCoordinate intersectionPoint;
if ( intersects(line, interatorLine, intersectionPoint) ){
intersectionList.append(intersectionPoint);
QPair<int, int> neighbours;
neighbours.first = i;
neighbours.second = area.nextVertexIndex(i);
neighbourList.append(neighbours);
}
}
if (intersectionList.count() > 0) {
return true;
} else {
return false;
}
} else {
qWarning("WimaArea::intersects(line, poly): line->count() != 2 || poly->count() < 3");
return false;
}
}
/*!other,
* \fn double WimaArea::distInsidePoly(const QGeoCoordinate &c1, const QGeoCoordinate &c2, WimaArea area)
* Returns the distance between the coordinate \a c1 and coordinate \a c2, or infinity if the shortest path between
* the two coordinates is not fully inside the \a area.
* \note Both coordinates must lie inside the \a area.
*
* \sa QGeoCoordinate
*/
double WimaArea::distInsidePoly(const QGeoCoordinate &c1, const QGeoCoordinate &c2, WimaArea area)
{
area.offset(0.1); // hack to compensate for numerical issues, migh be replaced in the future...
if ( area.containsCoordinate(c1) && area.containsCoordinate(c2)) {
QList<QGeoCoordinate> intersectionList;
QList<QPair<int, int>> neighbourlist;
QGCMapPolyline line;
line.appendVertex(c1);
line.appendVertex(c2);
intersects(line, area, intersectionList, neighbourlist);
if ( intersectionList.size() == 0 ){ // if an intersection was found the path between c1 and c2 is not fully inside area.
return c1.distanceTo(c2);
} else {
return std::numeric_limits<qreal>::infinity();
}
} else {
return std::numeric_limits<qreal>::infinity();
}
}
/*!
* \fn bool WimaArea::dijkstraPath(const QGeoCoordinate &start, const QGeoCoordinate &end, const WimaArea &area, QList<QGeoCoordinate> &dijkstraPath, QString &errorstring)
* Calculates the shortest path (inside \a area) between \a start and \a end.
* The \l {Dijkstra Algorithm} is used to find the shorest path.
* Stores the result inside \a dijkstraPath when sucessfull.
* Stores error messages in \a errorString.
* Returns \c true if successful, \c false else.
*
* \sa QList
*/
bool WimaArea::dijkstraPath(const QGeoCoordinate &start, const QGeoCoordinate &end, const WimaArea &area, QList<QGeoCoordinate> &dijkstraPath, QString &errorString) // don't seperate parameters with new lines or documentation will break
{
if ( isSelfIntersecting(area) ) {
errorString.append("Area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n");
return false;
}
// Each QGeoCoordinate gets stuff into a Node
/// @param distance is the distance between the Node and it's predecessor
struct Node{
QGeoCoordinate coordinate;
double distance = std::numeric_limits<qreal>::infinity();
Node* predecessorNode = nullptr;
};
// The list with all Nodes (start, end + poly.path())
QList<Node> nodeList;
// This list will be initalized with (pointer to) all elements of nodeList.
// Elements will be successively remove during the execution of the Dijkstra Algorithm.
QList<Node*> workingSet;
// initialize nodeList_maxAltitude
// start cooridnate
Node startNode;
startNode.coordinate = start;
startNode.distance = 0;
nodeList.append(startNode);
//poly cooridnates
for (int i = 0; i < area.count(); i++) {
Node node;
node.coordinate = area.vertexCoordinate(i);
nodeList.append(node);
}
//end coordinate
Node endNode;
endNode.coordinate = end;
nodeList.append(endNode);
// initialize working set
for (int i = 0; i < nodeList.size(); i++) {
Node* nodePtr = &nodeList[i];
workingSet.append(nodePtr);
}
// Dijkstra Algorithm
// https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
while (workingSet.size() > 0) {
// serach Node with minimal distance
double minDist = std::numeric_limits<qreal>::infinity();
int minDistIndex = 0;
for (int i = 0; i < workingSet.size(); i++) {
Node* node = workingSet.value(i);
double dist = node->distance;
if (dist < minDist) {
minDist = dist;
minDistIndex = i;
}
}
Node* u = workingSet.takeAt(minDistIndex);
//update distance
for (int i = 0; i < workingSet.size(); i++) {
Node* v = workingSet[i];
// is neighbour? dist == infinity if no neihbour
double dist = distInsidePoly(u->coordinate, v->coordinate, area);
// is ther a alternative path which is shorter?
double alternative = u->distance + dist;
if (alternative < v->distance) {
v->distance = alternative;
v->predecessorNode = u;
}
}
}
// end Djikstra Algorithm
// check it the Algorithm was sucessfulepsilonMeter
Node* Node = &nodeList.last();
if (Node->predecessorNode == nullptr) {
}
// reverse assemble path
while (1) {
dijkstraPath.prepend(Node->coordinate);
//Update Node
Node = Node->predecessorNode;
if (Node == nullptr) {
if (dijkstraPath[0].distanceTo(start) < epsilonMeter)// check if starting point was reached
break;
qWarning("WimaArea::dijkstraPath(): Error, no path found!\n");
return false;
}
}
return true;
}
/*!
* \fn bool WimaArea::isSelfIntersecting(const WimaArea &area)
* Returns \c true if the \a area is self intersecting, \c false else.
* \note If the \a area is self intersecting, it's not a \l {Simple Polygon}.
*/
bool WimaArea::isSelfIntersecting(const WimaArea &area)
{
int i = 0;
if (area.count() > 3) {
// check if any edge of the area (formed by two adjacent vertices) intersects with any other edge of the area
while(i < area.count()-1) {
QGeoCoordinate refBeginCoordinate = area.vertexCoordinate(i);
QGeoCoordinate refEndCoordinate = area.vertexCoordinate(area.nextVertexIndex(i));
QGCMapPolyline refLine;
refLine.appendVertex(refBeginCoordinate);
refLine.appendVertex(refEndCoordinate);
int j = area.nextVertexIndex(i);
while(j < area.count()) {
QGeoCoordinate intersectionPt;
QGCMapPolyline iteratorLine;
iteratorLine.appendVertex(area.vertexCoordinate(j));
iteratorLine.appendVertex(area.vertexCoordinate(area.nextVertexIndex(j)));
if ( intersects(refLine, iteratorLine, intersectionPt) ){
if ( !(intersectionPt.distanceTo(refBeginCoordinate) < epsilonMeter)
&& !(intersectionPt.distanceTo(refEndCoordinate) < epsilonMeter) ) {
return true;
}
}
j++;
}
i++;
}
}
return false;
}
/*!
* \fn bool WimaArea::isSelfIntersecting()
* Returns \c true if the calling area is self intersecting, \c false else.
* \note If the calling area is self intersecting, it's not a \l {Simple Polygon}.
*/
bool WimaArea::isSelfIntersecting()
bool WimaArea::isSimplePolygon()
{
return isSelfIntersecting(*this);
using namespace PolygonCalculus;
using namespace GeoUtilities;
if (this->count() > 2) {
QPolygonF polygon = toQPolygonF(toCartesian2D(this->coordinateList(), this->vertexCoordinate(0)));
return PolygonCalculus::isSimplePolygon(polygon);
} else
return false;
}
/*!
......
src/Wima/WimaArea.h
View file @ 426293a7
......@@ -11,6 +11,9 @@
#include "QGCGeo.h"
#include <QPair>
#include "GeoUtilities.h"
#include "PolygonCalculus.h"
class WimaArea : public QGCMapPolygon //abstract base class for all WimaAreas
{
Q_OBJECT
......@@ -46,18 +49,9 @@ public:
// static Methodes
static QGCMapPolygon toQGCPolygon (const WimaArea& area);
static bool join (WimaArea &area1, WimaArea &area2, WimaArea& joinedArea, QString &errorString);
static bool join (const WimaArea &area1, const WimaArea &area2, WimaArea& joinedArea, QString &errorString);
static bool join (WimaArea &area1, WimaArea &area2, WimaArea& joinedArea);
static bool intersects (const QGCMapPolyline& line1, const QGCMapPolyline& line2,
QGeoCoordinate& intersectionPt);
static bool intersects (const QGCMapPolyline& line, const WimaArea& area,
QList<QGeoCoordinate>& intersectionList,
QList<QPair<int, int>>& neighbourList);
static double distInsidePoly (const QGeoCoordinate& c1, const QGeoCoordinate& c2, WimaArea area);
static bool dijkstraPath (const QGeoCoordinate& c1, const QGeoCoordinate& c2,
const WimaArea& area, QList<QGeoCoordinate>& dijkstraPath, QString &errorstring);
static bool isSelfIntersecting (const WimaArea& area);
bool isSelfIntersecting ();
bool isSimplePolygon ();
// Friends
friend void print(const WimaArea& area, QString& outputString);
......
src/Wima/WimaPlaner.cc
View file @ 426293a7
#include "WimaPlaner.h"
const char* WimaPlaner::wimaFileExtension = "wima";
const char* WimaPlaner::areaItemsName = "AreaItems";
const char* WimaPlaner::missionItemsName = "MissionItems";
......@@ -253,10 +255,8 @@ bool WimaPlaner::updateMission()
QGeoCoordinate start = _serviceArea.center();
QGeoCoordinate end = survey->visualTransectPoints().first().value<QGeoCoordinate>();
QList<QGeoCoordinate> path;
if ( !WimaArea::dijkstraPath(start, end, _joinedArea, path, errorString)) {
qgcApp()->showMessage(QString( QString(tr("Not able to calculate the path from takeoff position to measurement area."))
+ errorString
).toLocal8Bit().data());
if ( !calcShortestPath(start, end, path)) {
qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data());
return false;
}
for (int i = 1; i < path.count()-1; i++) {
......@@ -268,10 +268,8 @@ bool WimaPlaner::updateMission()
start = survey->visualTransectPoints().last().value<QGeoCoordinate>();
end = _serviceArea.center();
path.clear();
if ( ! WimaArea::dijkstraPath(start, end, _joinedArea, path, errorString)) {
qgcApp()->showMessage(QString( QString(tr("Not able to calculate the path from measurement area to landing position."))
+ errorString
).toLocal8Bit().data());
if ( !calcShortestPath(start, end, path)) {
qgcApp()->showMessage(QString(tr("Not able to calculate the path from measurement area to landing position.")).toLocal8Bit().data());
return false;
}
for (int i = 1; i < path.count()-1; i++) {
......@@ -500,22 +498,24 @@ void WimaPlaner::recalcPolygonInteractivity(int index)
bool WimaPlaner::recalcJoinedArea(QString &errorString)
{
// check if area paths form simple polygons
if ( WimaArea::isSelfIntersecting(_serviceArea) ) {
if ( !_serviceArea.isSimplePolygon() ) {
errorString.append(tr("Service area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
return false;
}
if ( WimaArea::isSelfIntersecting(_corridor) ) {
if ( !_corridor.isSimplePolygon() && _corridor.count() > 0) {
errorString.append(tr("Corridor is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
return false;
}
if ( WimaArea::isSelfIntersecting(_measurementArea) ) {
if ( !_measurementArea.isSimplePolygon() ) {
errorString.append(tr("Measurement area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
return false;
}
// join service area, op area and corridor
if (!_visualItems.contains(&_joinedArea))
_visualItems.append(&_joinedArea);
_joinedArea.setPath(_serviceArea.path());
_joinedArea.join(_corridor);
if ( !_joinedArea.join(_measurementArea) ) {
......@@ -547,6 +547,21 @@ void WimaPlaner::pushToContainer()
}
}
bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QList<QPointF> path2D;
bool retVal = PolygonCalculus::shortestPath(
toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/start)),
/*start point*/ QPointF(0,0),
/*destination*/toCartesian2D(destination, start),
/*shortest path*/path2D);
path.append(toGeo(path2D, /*origin*/start));
return retVal;
}
void WimaPlaner::resetAllInteractive()
{
// Marks all areas as inactive (area.interactive == false)
......
src/Wima/WimaPlaner.h
View file @ 426293a7
......@@ -25,6 +25,10 @@
#include "JsonHelper.h"
#include "QGCApplication.h"
#include "OptimisationTools.h"
#include "PlanimetryCalculus.h"
#include "GeoUtilities.h"
class WimaPlaner : public QObject
{
......@@ -117,6 +121,7 @@ private slots:
void recalcPolygonInteractivity (int index);
bool recalcJoinedArea (QString &errorString);
void pushToContainer ();
bool calcShortestPath (const QGeoCoordinate &start, const QGeoCoordinate &destination, QList<QGeoCoordinate> &path);
private:
// Member Functions
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment