Skip to content

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

join bug fixed

parent 426293a7
Hide whitespace changes
Inline Side-by-side
Showing with 131 additions and 58 deletions
src/Wima/OptimisationTools.cc
View file @ dc7b935d
......@@ -85,7 +85,7 @@ namespace OptimisationTools {
Node* node = &nodeList[endIndex];
while (1) {
if (node == nullptr) {
if (elementPath[0] == elementPath[startIndex])// check if starting point was reached
if (elementPath[0] == elements[startIndex])// check if starting point was reached
break;
return false;
}
......
src/Wima/PlanimetryCalculus.cc
View file @ dc7b935d
......@@ -318,6 +318,13 @@ angle
double x2 = line2L1.x2();
double y1 = line2L1.y1();
double y2 = line2L1.y2();
if (x1 > x2) {
x1 = x2;
x2 = line2L1.x1();
y1 = y2;
y2 = line2L1.y1();
}
double dx = (x2-x1);
double dy = (y2-y1);
double xNull = 0; // (xNull, 0) intersection point in line1 system
......@@ -548,6 +555,68 @@ angle
return angle(line.p1(), line.p2());
}
bool contains(const QLineF &line, const QPointF &point, IntersectType &type)
{
QPointF translationVector = line.p1();
double alpha = angle(line);
double l = line.length();
QPointF pointL1 = rotateReturn(point - translationVector, -alpha);
double x = pointL1.x();
double y = pointL1.y();
if ( x >= 0 && x <= l ) {
if (qFuzzyIsNull(x) || qFuzzyCompare(x, l)) {
if (qFuzzyIsNull(y))
{
type = CornerCornerIntersection;
return true;
}
} else {
if (qFuzzyIsNull(y))
{
type = EdgeCornerIntersection;
return true;
}
}
}
type = NoIntersection;
return false;
}
bool contains(const QLineF &line, const QPointF &point)
{
IntersectType dummyType;
return contains(line, point, dummyType);
}
bool contains(const QPolygonF &polygon, const QPointF &point, IntersectType &type)
{
using namespace PolygonCalculus;
if (polygon.containsPoint(point, Qt::FillRule::OddEvenFill))
{
type = Interior;
return true;
}
int size = polygon.size();
for (int i = 0; i < size; i++) {
QLineF line(polygon[i], polygon[nextVertexIndex(size, i)]);
if ( contains(line, point, type) ) {
return true;
}
}
return false;
}
bool contains(const QPolygonF &polygon, const QPointF &point)
{
IntersectType dummyType;
return contains(polygon, point, dummyType);
}
......
src/Wima/PlanimetryCalculus.h
View file @ dc7b935d
......@@ -21,6 +21,8 @@ namespace PlanimetryCalculus {
EdgeCornerIntersection, EdgeEdgeIntersection, CornerCornerIntersection,
LinesParallel, LinesEqual, // Line Line intersection
Interior, // Polygon contains
NoIntersection, Error // general
};
......@@ -55,6 +57,12 @@ namespace PlanimetryCalculus {
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList, NeighbourList &neighbourList);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList, NeighbourList &neighbourList, IntersectList &typeList);
bool contains(const QLineF &line, const QPointF &point);
bool contains(const QLineF &line, const QPointF &point, IntersectType &type);
bool contains(const QPolygonF &polygon, const QPointF &point);
bool contains(const QPolygonF &polygon, const QPointF &point, IntersectType &type);
double distance(const QPointF &p1, const QPointF p2);
double angle(const QPointF &p1, const QPointF p2);
double angle(const QLineF &line);
......
src/Wima/PolygonCalculus.cc
View file @ dc7b935d
......@@ -145,7 +145,7 @@ namespace PolygonCalculus {
*/
JoinPolygonError join(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon)
{
using namespace PolygonCalculus;
using namespace PlanimetryCalculus;
if (polygon1.size() >= 3 && polygon2.size() >= 3) {
if ( !isSimplePolygon(polygon1) || !isSimplePolygon(polygon2)) {
......@@ -178,13 +178,12 @@ namespace PolygonCalculus {
joinedPolygon.append(*crossPoly);
return JoinPolygonError::PolygonJoined;
}
QPointF lastVertex = walkerPoly->last();
QPointF currentVertex = walkerPoly->value(startIndex);
QPointF startVertex = currentVertex;
// possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly)
int nextVertexNumber = nextVertexIndex(walkerPoly->size(), startIndex);
QPointF protoNextVertex = walkerPoly->value(nextVertexNumber);
bool switchHappenedPreviously = false; // means switch between crossPoly and walkerPoly
while (1) {
//qDebug("nextVertexNumber: %i", nextVertexNumber);
joinedPolygon.append(currentVertex);
......@@ -200,32 +199,22 @@ namespace PolygonCalculus {
//qDebug("IntersectionList.size(): %i", intersectionList.size());
if (intersectionList.size() >= 1) {
int minDistIndex = 0;
if (intersectionList.size() > 0) {
int minDistIndex = -1;
// find the vertex with the least distance to currentVertex
if (intersectionList.size() > 1) {
double minDist = PlanimetryCalculus::distance(currentVertex, intersectionList[minDistIndex]);
for (int i = 1; i < intersectionList.size(); i++) {
double currentDist = PlanimetryCalculus::distance(currentVertex, intersectionList[i]);
double minDist = std::numeric_limits<double>::infinity();
for (int i = 0; i < intersectionList.size(); i++) {
double currentDist = PlanimetryCalculus::distance(currentVertex, intersectionList[i]);
if ( minDist > currentDist ) {
minDist = currentDist;
minDistIndex = i;
}
if ( minDist > currentDist && currentVertex != intersectionList[i]) {
minDist = currentDist;
minDistIndex = i;
}
}
//qDebug("MinDistIndex: %i", minDistIndex);
QPointF protoCurrentVertex = intersectionList.value(minDistIndex);
// If the currentVertex is a intersection point a intersection ocisSelfIntersectingcures with the
// crossPoly. This would cause unwanted switching of crossPoly and walkerPoly, thus intersections
// are only token in to account if they occur beyond a certain distance (_epsilonMeter) or no switching happend in the
// previous step.
if (switchHappenedPreviously == false){
//|| protoCurrentVertex.distanceTo(currentVertex) > _epsilonMeter) {
currentVertex = protoCurrentVertex;
if (minDistIndex != -1){
lastVertex = currentVertex;
currentVertex = intersectionList.value(minDistIndex);
QPair<int, int> neighbours = neighbourList.value(minDistIndex);
protoNextVertex = crossPoly->value(neighbours.second);
nextVertexNumber = neighbours.second;
......@@ -234,33 +223,35 @@ namespace PolygonCalculus {
const QPolygonF *temp = walkerPoly;
walkerPoly = crossPoly;
crossPoly = temp;
switchHappenedPreviously = true;
} else {
currentVertex = walkerPoly->value(nextVertexNumber);
lastVertex = currentVertex;
currentVertex = walkerPoly->value(nextVertexNumber);
nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber);
protoNextVertex = walkerPoly->value(nextVertexNumber);
switchHappenedPreviously = false;
protoNextVertex = walkerPoly->value(nextVertexNumber);
}
} else {
currentVertex = walkerPoly->value(nextVertexNumber);
lastVertex = currentVertex;
currentVertex = walkerPoly->value(nextVertexNumber);
nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber);
protoNextVertex = walkerPoly->value(nextVertexNumber);
protoNextVertex = walkerPoly->value(nextVertexNumber);
}
if (currentVertex == startVertex) {
if (polygon1.size() == joinedPolygon.size()) {
return JoinPolygonError::Disjoint;
for (int i = 0; i < polygon1.size(); i++) {
if (polygon1[i] != joinedPolygon[i])
return PolygonJoined;
}
return Disjoint;
} else {
return JoinPolygonError::PolygonJoined;
return PolygonJoined;
}
}
}
} else {
return JoinPolygonError::PathSizeLow;
return PathSizeLow;
}
}
......@@ -488,10 +479,12 @@ namespace PolygonCalculus {
return;
}
bool shortestPath(const QPolygonF &polygon, const QPointF &startVertex, const QPointF &endVertex, QList<QPointF> &shortestPath)
bool shortestPath(QPolygonF polygon, QPointF startVertex, const QPointF &endVertex, QList<QPointF> &shortestPath)
{
if ( polygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill)
&& polygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) {
using namespace PlanimetryCalculus;
offsetPolygon(polygon, 0.1);
if ( contains(polygon, startVertex)
&& contains(polygon, endVertex)) {
// lambda
std::function<double(const QPointF &, const QPointF &)> distance = [polygon](const QPointF &p1, const QPointF &p2) -> double {
if (containsPath(polygon, p1, p2)){
......
src/Wima/PolygonCalculus.h
View file @ dc7b935d
......@@ -25,7 +25,7 @@ namespace PolygonCalculus {
void offsetPolygon (QPolygonF &polygon, double offset);
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);
bool shortestPath (QPolygonF polygon, QPointF startVertex, const QPointF &endVertex, QList<QPointF> &shortestPath);
QPolygonF toQPolygonF(const QVector3DList &polygon);
QPolygonF toQPolygonF(const QPointFList &polygon);
......
src/Wima/WimaArea.cc
View file @ dc7b935d
......@@ -147,31 +147,31 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &join
QList<QGeoCoordinate> GeoPolygon1 = area1.coordinateList();
QList<QGeoCoordinate> GeoPolygon2 = area2.coordinateList();
/*qWarning("befor joining");
qWarning("befor joining");
qWarning() << GeoPolygon1;
qWarning() << GeoPolygon2;*/
qWarning() << GeoPolygon2;
QGeoCoordinate origin = GeoPolygon1[0];
QGeoCoordinate tset = GeoPolygon1[2];
//qWarning() << tset;qWarning() << toGeo(toCartesian2D(tset, origin), origin);
qWarning() << tset;qWarning() << toGeo(toCartesian2D(tset, origin), origin);
QPolygonF polygon1 = toQPolygonF(toCartesian2D(GeoPolygon1, origin));
QPolygonF polygon2 = toQPolygonF(toCartesian2D(GeoPolygon2, origin));
/*qWarning("after 1 transform");
qWarning("after 1 transform");
qWarning() << polygon1;
qWarning() << polygon2;*/
qWarning() << polygon2;
QPolygonF joinedPolygon;
JoinPolygonError retValue = PolygonCalculus::join(polygon1, polygon2, joinedPolygon);
/*qWarning("after joining");
qWarning() << joinedPolygon;*/
qWarning("after joining");
qWarning() << joinedPolygon;
if (retValue == JoinPolygonError::Disjoint) {
qWarning("Polygons are disjoint.");
......@@ -181,8 +181,8 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &join
qWarning("Polygon vertex count is low.");
} else {
QList<QGeoCoordinate> path = toGeo(toQPointFList(joinedPolygon), origin);
//qWarning("after transform");
//qWarning() << path;
qWarning("after transform");
qWarning() << path;
joinedArea.setPath(path);
return true;
}
......@@ -199,7 +199,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &join
* 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)
bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &joinedArea)
{
QString dummy;
return join(area1, area2, joinedArea, dummy);
......@@ -217,6 +217,8 @@ bool WimaArea::join(WimaArea &area)
{
WimaArea joinedArea;
if ( join(*this, area, joinedArea) ) {
//qWarning("WimaArea::join(WimaArea &area)");
//qWarning() << joinedArea.coordinateList();
this->setPath(joinedArea.path());
return true;
} else {
......
src/Wima/WimaArea.h
View file @ dc7b935d
......@@ -50,7 +50,7 @@ public:
// static Methodes
static QGCMapPolygon toQGCPolygon (const WimaArea& area);
static bool join (const WimaArea &area1, const WimaArea &area2, WimaArea& joinedArea, QString &errorString);
static bool join (WimaArea &area1, WimaArea &area2, WimaArea& joinedArea);
static bool join (const WimaArea &area1, const WimaArea &area2, WimaArea& joinedArea);
bool isSimplePolygon ();
// Friends
......
src/Wima/WimaPlaner.cc
View file @ dc7b935d
......@@ -513,21 +513,22 @@ bool WimaPlaner::recalcJoinedArea(QString &errorString)
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) ) {
errorString.append(tr("Not able to join areas. Service area and measurement are"
errorString.append(tr("Not able to join areas. Service area and measurement"
" must have a overlapping section, or be connected through a corridor."));
return false; // this happens if all areas are pairwise disjoint
}
else {
return true;
}
// join service area, op area and corridor
// remove if debugging finished
WimaServiceArea *test = new WimaServiceArea(this);
test->setPath(_joinedArea.path());
_visualItems.append(test);
return true;
}
/*!
......
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