#include "PolygonCalculus.h"
#include "PlanimetryCalculus.h"
#include "OptimisationTools.h"
#include <QVector3D>
#include <functional>
namespace PolygonCalculus {
namespace {
bool isReflexVertex(const QPolygonF& polygon, const QPointF *vertex) {
// Original Code from SurveyComplexItem::_VertexIsReflex()
auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
auto area = ( ((vertex->x() - vertexBefore->x())*(vertexAfter->y() - vertexBefore->y()))
-((vertexAfter->x() - vertexBefore->x())*(vertex->y() - vertexBefore->y())));
return area > 0;
}
} // end anonymous namespace
/*!
* \fn bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2)
* Returns true if the shortest path between the two coordinates is not fully inside the \a area.
*
* \sa QPointF, QPolygonF
*/
bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2)
{
if ( !polygon.isEmpty()) {
if ( !polygon.containsPoint(c1, Qt::FillRule::OddEvenFill)
|| !polygon.containsPoint(c2, Qt::FillRule::OddEvenFill))
return false;
QVector<QPointF> intersectionList;
QLineF line;
line.setP1(c1);
line.setP2(c2);
PlanimetryCalculus::IntersectVector intersectTypeList;
bool retValue = PlanimetryCalculus::intersects(polygon, line, intersectionList, intersectTypeList);
if (retValue) {
for (int i = 0; i < intersectTypeList.size(); i++) {
PlanimetryCalculus::IntersectType type = intersectTypeList[i];
if ( type == PlanimetryCalculus::EdgeEdgeIntersection
|| type == PlanimetryCalculus::Error)
return false;
}
}
return true;
} else {
return false;
}
}
/*!
* \fn int closestVertexIndex(const QPolygonF &polygon, const QPointF &coordinate)
* Returns the vertex index of \a polygon which has the least distance to \a coordinate.
*
* \sa QPointF, QPolygonF
*/
int closestVertexIndex(const QPolygonF &polygon, const QPointF &coordinate)
{
if (polygon.size() == 0) {
qWarning("Path is empty!");
return -1;
}else if (polygon.size() == 1) {
return 0;
}else {
int index = 0; // the index of the closest vertex
double min_dist = PlanimetryCalculus::distance(coordinate, polygon[index]);
for(int i = 1; i < polygon.size(); i++){
double dist = PlanimetryCalculus::distance(coordinate, polygon[i]);
if (dist < min_dist){
min_dist = dist;
index = i;
}
}
return index;
}
}
/*!auto distance
* \fn QPointF closestVertex(const QPolygonF &polygon, const QPointF &coordinate);
* Returns the vertex of \a polygon with the least distance to \a coordinate.
*
* \sa QPointF, QPolygonF
*/
QPointF closestVertex(const QPolygonF &polygon, const QPointF &coordinate)
{
int index = closestVertexIndex(polygon, coordinate);
if (index >=0 ) {
return polygon[index];
} else {
return QPointF();
}
}
/*!
* \fn int nextPolygonIndex(int pathsize, int index)
* Returns the index of the next vertex (of a polygon), which is \a index + 1 if \a index is smaller than \c {\a pathsize - 1},
* or 0 if \a index equals \c {\a pathsize - 1}, or -1 if the \a index is out of bounds.
* \note \a pathsize is usually obtained by invoking polygon.size()
*/
int nextVertexIndex(int pathsize, int index)
{
if (index >= 0 && index < pathsize-1) {
return index + 1;
} else if (index == pathsize-1) {
return 0;
} else {
qWarning("nextPolygonIndex(): Index out of bounds! index:count = %i:%i", index, pathsize);
return -1;
}
}
/*!
* \fn int previousPolygonIndex(int pathsize, int index)
* Returns the index of the previous vertex (of a polygon), which is \a index - 1 if \a index is larger 0,
* or \c {\a pathsize - 1} if \a index equals 0, or -1 if the \a index is out of bounds.
* \note pathsize is usually obtained by invoking polygon.size()
*/
int previousVertexIndex(int pathsize, int index)
{
if (index > 0 && index <pathsize) {
return index - 1;
} else if (index == 0) {
return pathsize-1;
} else {
qWarning("previousVertexIndex(): Index out of bounds! index:count = %i:%i", index, pathsize);
return -1;
}
}
/*!
* \fn JoinPolygonError joinPolygon(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon);
* Joins \a polygon1 and \a polygon2 such that a \l {Simple Polygon} is created.
* Stores the result inside \a joinedArea.
* Returns \c NotSimplePolygon1 if \a polygon1 isn't a Simple Polygon, \c NotSimplePolygon2 if \a polygon2 isn't a Simple Polygon, \c Disjoind if the polygons are disjoint,
* \c PathSizeLow if at least one polygon has a size samler than 3, or \c PolygonJoined on success.
* The algorithm will be able to join the areas, if either their edges intersect with each other,
* or one area is inside the other.
* The algorithm assumes that \a joinedPolygon is empty.
*/
JoinPolygonError join(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon)
{
using namespace PlanimetryCalculus;
if (polygon1.size() >= 3 && polygon2.size() >= 3) {
if ( !isSimplePolygon(polygon1) || !isSimplePolygon(polygon2)) {
return JoinPolygonError::NotSimplePolygon;
}
if ( !hasClockwiseWinding(polygon1)) {
reversePath(polygon1);
}
if ( !hasClockwiseWinding(polygon2)) {
reversePath(polygon2);
}
const QPolygonF *walkerPoly = &polygon1; // "walk" on this polygon towards higher indices
const QPolygonF *crossPoly = &polygon2; // 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->size(); i++) {
if ( !contains(*crossPoly, walkerPoly->at(i)) ) {
crossContainsWalker = false;
startIndex = i;
break;
}
}
if ( crossContainsWalker == true) {
joinedPolygon.append(*crossPoly);
return JoinPolygonError::PolygonJoined;
}
QPointF currentVertex = walkerPoly->at(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);
while (1) {
//qDebug("nextVertexNumber: %i", nextVertexNumber);
joinedPolygon.append(currentVertex);
QLineF walkerPolySegment;
walkerPolySegment.setP1(currentVertex);
walkerPolySegment.setP2(protoNextVertex);
QVector<QPair<int, int>> neighbourList;
QVector<QPointF> intersectionList;
//qDebug("IntersectionList.size() on init: %i", intersectionList.size());
PlanimetryCalculus::intersects(*crossPoly, walkerPolySegment, intersectionList, neighbourList);
//qDebug("IntersectionList.size(): %i", intersectionList.size());
if (intersectionList.size() > 0) {
int minDistIndex = -1;
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 && !qFuzzyIsNull(distance(currentVertex, intersectionList[i])) ) {
minDist = currentDist;
minDistIndex = i;
}
}
if (minDistIndex != -1){
currentVertex = intersectionList.value(minDistIndex);
QPair<int, int> neighbours = neighbourList.value(minDistIndex);
protoNextVertex = crossPoly->value(neighbours.second);
nextVertexNumber = neighbours.second;
// switch walker and cross poly
const QPolygonF *temp = walkerPoly;
walkerPoly = crossPoly;
crossPoly = temp;
} else {
currentVertex = walkerPoly->value(nextVertexNumber);
nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber);
protoNextVertex = walkerPoly->value(nextVertexNumber);
}
} else {
currentVertex = walkerPoly->value(nextVertexNumber);
nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber);
protoNextVertex = walkerPoly->value(nextVertexNumber);
}
if (currentVertex == startVertex) {
if (polygon1.size() == joinedPolygon.size()) {
for (int i = 0; i < polygon1.size(); i++) {
if (polygon1[i] != joinedPolygon[i])
return PolygonJoined;
}
return Disjoint;
} else {
return PolygonJoined;
}
}
}
} else {
return PathSizeLow;
}
}
/*!
* \fn bool isSimplePolygon(const QPolygonF &polygon);
* Returns \c true if \a polygon is a \l {Simple Polygon}, \c false else.
* \note A polygon is a Simple Polygon iff it is not self intersecting.
*/
bool isSimplePolygon(const QPolygonF &polygon)
{
int i = 0;
if (polygon.size() > 3) {
// check if any edge of the area (formed by two adjacent vertices) intersects with any other edge of the area
while(i < polygon.size()-1) {
double cCIntersectCounter = 0; // corner corner intersection counter
QPointF refBeginCoordinate = polygon[i];
QPointF refEndCoordinate = polygon[nextVertexIndex(polygon.size(), i)];
QLineF refLine;
refLine.setP1(refBeginCoordinate);
refLine.setP2(refEndCoordinate);
int j = nextVertexIndex(polygon.size(), i);
while(j < polygon.size()) {
QPointF intersectionPt;
QLineF iteratorLine;
iteratorLine.setP1(polygon[j]);
iteratorLine.setP2(polygon[nextVertexIndex(polygon.size(), j)]);
PlanimetryCalculus::IntersectType intersectType;
PlanimetryCalculus::intersects(refLine, iteratorLine, intersectionPt, intersectType);
if ( intersectType == PlanimetryCalculus::CornerCornerIntersection) {
cCIntersectCounter++;
// max two corner corner intersections allowed, a specific coordinate can appear only once in a simple polygon
}
if ( cCIntersectCounter > 2
|| intersectType == PlanimetryCalculus::EdgeEdgeIntersection
|| intersectType == PlanimetryCalculus::EdgeCornerIntersection
|| intersectType == PlanimetryCalculus::LinesEqual
|| intersectType == PlanimetryCalculus::Error){
return false;
}
j++;
}
i++;
}
}
return true;
}
/*!
* \fn bool hasClockwiseWinding(const QPolygonF &polygon)
* Returns \c true if \a path has clockwiauto distancese winding, \c false else.
*/
bool hasClockwiseWinding(const QPolygonF &polygon)
{
if (polygon.size() <= 2) {
return false;
}
double sum = 0;
for (int i=0; i <polygon.size(); i++) {
QPointF coord1 = polygon[i];
QPointF coord2 = polygon[nextVertexIndex(polygon.size(), i)];
sum += (coord2.x() - coord1.x()) * (coord2.y() + coord1.y());
}
if (sum < 0.0)
return false;
return true;
}
/*!
* \fn void reversePath(QPolygonF &polygon)
* Reverses the path, i.e. changes the first Vertex with the last, the second with the befor last and so forth.
*/
void reversePath(QPolygonF &polygon)
{
QPolygonF pathReversed;
for (int i = 0; i < polygon.size(); i++) {
pathReversed.prepend(polygon[i]);
}
polygon.clear();
polygon.append(pathReversed);
}
/*!
* \fn void offsetPolygon(QPolygonF &polygon, double offset)
* Offsets a \a polygon by the given \a offset. The algorithm assumes that polygon is a \l {SimplePolygon}.
*/
void offsetPolygon(QPolygonF &polygon, double offset)
{
// Original code from QGCMapPolygon::offset()
QPolygonF newPolygon;
if (polygon.size() > 2) {
// 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);
QLineF offsetEdge;
QLineF originalEdge(polygon[i], polygon[nextIndex]);
QLineF workerLine = originalEdge;
workerLine.setLength(offset);
workerLine.setAngle(workerLine.angle() - 90.0);
offsetEdge.setP1(workerLine.p2());
workerLine.setPoints(originalEdge.p2(), originalEdge.p1()); bool containsPath (const QPointF &c1, const QPointF &c2, QPolygonF polygon);
workerLine.setLength(offset);
workerLine.setAngle(workerLine.angle() + 90.0);
offsetEdge.setP2(workerLine.p2());
rgOffsetEdges << offsetEdge;
}
// Intersect the offset edges to generate new vertices
polygon.clear();
QPointF newVertex;
for (int i=0; i<rgOffsetEdges.count(); i++) {
int prevIndex = previousVertexIndex(rgOffsetEdges.size(), i);
if (rgOffsetEdges[prevIndex].intersect(rgOffsetEdges[i], &newVertex) == QLineF::NoIntersection) {
// FIXME: Better error handling?
qWarning("Intersection failed");
return;
}
polygon.append(newVertex);
}
}
}
void decomposeToConvex(const QPolygonF &polygon, QList<QPolygonF> &convexPolygons)
{
// Original Code SurveyComplexItem::_PolygonDecomposeConvex()
// this follows "Mark Keil's Algorithm" https://mpen.ca/406/keil
int decompSize = std::numeric_limits<int>::max();
if (polygon.size() < 3) return;
if (polygon.size() == 3) {
convexPolygons << polygon;
return;
}
QList<QPolygonF> decomposedPolygonsMin{};
for (const QPointF *vertex = polygon.begin(); vertex != polygon.end(); ++vertex)
{
// is vertex reflex?
bool vertexIsReflex = isReflexVertex(polygon, vertex);
if (!vertexIsReflex) continue;
for (const QPointF *vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther)
{
const QPointF *vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
const QPointF *vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
if (vertexOther == vertex) continue;
if (vertexAfter == vertexOther) continue;
if (vertexBefore == vertexOther) continue;
bool canSee = containsPath(polygon, *vertex, *vertexOther);
if (!canSee) continue;
QPolygonF polyLeft;
const QPointF *v = vertex;
bool polyLeftContainsReflex = false;
while ( v != vertexOther) {
if (v != vertex && isReflexVertex(polygon, v)) {
polyLeftContainsReflex = true;
}
polyLeft << *v;
++v;
if (v == polygon.end()) v = polygon.begin();
}
polyLeft << *vertexOther;
bool polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3);
QPolygonF polyRight;
v = vertexOther;
bool polyRightContainsReflex = false;
while ( v != vertex) {
if (v != vertex && isReflexVertex(polygon, v)) {
polyRightContainsReflex = true;
}
polyRight << *v;
++v;
if (v == polygon.end()) v = polygon.begin();
}
polyRight << *vertex;
auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3);
if (!polyLeftValid || ! polyRightValid) {
// decompSize = std::numeric_limits<int>::max();
continue;
}
// recursion
QList<QPolygonF> polyLeftDecomposed{};
decomposeToConvex(polyLeft, polyLeftDecomposed);
QList<QPolygonF> polyRightDecomposed{};
decomposeToConvex(polyRight, polyRightDecomposed);
// compositon
int subSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
if ( (polyLeftContainsReflex && polyLeftDecomposed.size() == 1)
|| (polyRightContainsReflex && polyRightDecomposed.size() == 1))
{
// don't accept polygons that contian reflex vertices and were not split
subSize = std::numeric_limits<int>::max();
}
if (subSize < decompSize) {
decompSize = subSize;
decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
}
}
}
// assemble output
if (decomposedPolygonsMin.size() > 0) {
convexPolygons << decomposedPolygonsMin;
} else {
convexPolygons << polygon;
}
return;
}
bool shortestPath(QPolygonF polygon,const QPointF &startVertex, const QPointF &endVertex, QVector<QPointF> &shortestPath)
{
using namespace PlanimetryCalculus;
offsetPolygon(polygon, 0.01); // solves numerical errors
if ( contains(polygon, startVertex)
&& contains(polygon, endVertex)) {
// lambda
QPolygonF polygon2 = polygon;
offsetPolygon(polygon2, 0.01); // solves numerical errors
QVector<QPointF> elementList;
elementList.append(startVertex);
elementList.append(endVertex);
for (auto vertex : polygon) elementList.append(vertex);
const int listSize = elementList.size();
std::function<double(const int, const int)> distanceDij = [polygon2, elementList](const int ind1, const int ind2) -> double {
QPointF p1 = elementList[ind1];
QPointF p2 = elementList[ind2];
double dist = std::numeric_limits<double>::infinity();
if (containsPath(polygon2, p1, p2)){
double dx = p1.x()-p2.x();
double dy = p1.y()-p2.y();
dist = (dx*dx)+(dy*dy); // sqrt not necessary
}
return dist;
};
QVector<int> shortestPathIndex;
bool retVal = OptimisationTools::dijkstraAlgorithm(listSize, 0, 1, shortestPathIndex, distanceDij);
for (auto index : shortestPathIndex) shortestPath.append(elementList[index]);
return retVal;
} 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;
}
void reversePath(QPointFList &path)
{
QPointFList pathReversed;
for ( auto element : path) {
pathReversed.prepend(element);
}
path = pathReversed;
}
void reversePath(QPointFVector &path)
{
QPointFVector pathReversed;
for ( auto element : path) {
pathReversed.prepend(element);
}
path = pathReversed;
}
} // end PolygonCalculus namespace