WimaArea.cc

#include "WimaArea.h"

const double WimaArea::numericalAccuracy = 1e-3; // meters

WimaArea::WimaArea(QObject *parent) :
    WimaArea           (nullptr, parent)
{
    //qWarning() << "WimaPolygon:: polygon count" << _polygon->count();
}

WimaArea::WimaArea(QGCMapPolygon *other, QObject *parent)
    : QGCMapPolygon (other, parent)
{
    WimaArea* wimaPoly = qobject_cast<WimaArea*>(other);
    if (wimaPoly != nullptr) {
        _maxAltitude = wimaPoly->maxAltitude();
        _wimaVehicle = wimaPoly->vehicle();
    } else {
        _maxAltitude = 30;
        _wimaVehicle = new WimaVehicle(this);
    }
}





void WimaArea::setMaxAltitude(double alt)
{
    if(alt > 0 && alt != _maxAltitude){
        _maxAltitude = alt;
        emit maxAltitudeChanged();
    }
}


void WimaArea::setVehicle(WimaVehicle *vehicle)
{
    if(_wimaVehicle != vehicle){
        _wimaVehicle->deleteLater();
        _wimaVehicle =  vehicle;
        emit vehicleChanged();
    }
}

/*QList<WimaArea *>* WimaArea::splitArea<T>(WimaArea *polygonToSplitt, int numberOfFractions)
{
    if(numberOfFractions > 0 && polygonToSplitt != nullptr){
        WimaArea* poly;
        if(p)
        = new WimaArea(polygonToSplitt, this);
        QList<WimaArea*>* list = new QList<WimaArea*>();
        list->append(poly);
        return list;
    }
    return nullptr;
}


QList<QGCMapPolygon*>* WimaArea::splitArea(int numberOfFractions)
{
    return splitPolygonArea(this, numberOfFractions);
}*/

int WimaArea::getClosestVertexIndex(QGeoCoordinate coordinate)
{
    if (this->count() == 0) {
        qWarning("Polygon count == 0!");
        return -1;
    }else if (this->count() == 1) {
        return 0;
    }else {
        int index = 0;
        double min_dist = coordinate.distanceTo(this->vertexCoordinate(index));
        for(int i = 1; i < this->count(); i++){
            double dist = coordinate.distanceTo(this->vertexCoordinate(i));
            if (dist < min_dist){
                min_dist = dist;
                index = i;
            }
        }

        return index;
    }
}

QGeoCoordinate WimaArea::getClosestVertex(QGeoCoordinate coordinate)
{
    return this->vertexCoordinate(getClosestVertexIndex(coordinate));
}

QGCMapPolygon* WimaArea::toQGCPolygon(WimaArea *poly)
{
    if (poly != nullptr) {
        QGCMapPolygon* qgcPoly = new QGCMapPolygon(this);
        qgcPoly->setPath(poly->path());
        qgcPoly->setCenter(poly->center());
        qgcPoly->setCenterDrag(poly->centerDrag());
        qgcPoly->setInteractive(poly->interactive());

        return qgcPoly;
    } else {
        qWarning("WimaArea::toQGCPolygon(): poly == nullptr");
        return nullptr;
    }
}

void WimaArea::join(QList<WimaArea *>* polyList,  WimaArea* joinedPoly)
{
    return;
}


void WimaArea::join(WimaArea *poly1, WimaArea *poly2,  WimaArea* joinedPoly)
{
    if (poly1 != nullptr && poly2 != nullptr) {
        if (poly1->count() >= 3 && poly2->count() >= 3) {

            poly1->verifyClockwiseWinding();
            poly2->verifyClockwiseWinding();

            WimaArea* walkerPoly    = poly1; // "walk" on this polygon towards higher indices
            WimaArea* crossPoly     = poly2; // 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;
                }
            }


            if ( crossContainsWalker == true) {
                joinedPoly->appendVertices(crossPoly->coordinateList());
                return;
            }


            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);
                joinedPoly->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) > WimaArea::numericalAccuracy) {
                        currentVertex                   = protoCurrentVertex;
                        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) {
                    return;
                }
            }

        } else {
            qWarning("WimaArea::joinPolygons(poly1, poly2): poly->count() < 3");
            return;
        }

    } else {
        qWarning("WimaArea::joinPolygons(poly1, poly2): nullptr");
        return;
    }
}
void WimaArea::join(WimaArea *poly)
{
    WimaArea joinedArea(this);
    join(this, poly, &joinedArea);
    this->setPath(joinedArea.path());
    return;
}

bool WimaArea::isDisjunct(QList<WimaArea *>* polyList)
{
    // needs improvement
    if (polyList != nullptr){
        for (int i = 0;i < polyList->size()-1; i++) {
            WimaArea* currPoly = polyList->value(i);
            for (int j = i+1; i < polyList->size(); j++) {
                if (isDisjunct(currPoly, polyList->value(j))) {
                    return false;
                }
            }
        }
        return true;
    } else {
        qWarning("WimaArea::isDisjunct(polyList): polyList == nullptr!");
        return false;
    }
}

bool WimaArea::isDisjunct(WimaArea *poly1, WimaArea *poly2)
{
    if (poly1 != nullptr && poly2 != nullptr) {
        QGCMapPolygon* poly1Copy = new QGCMapPolygon(this);
        poly1Copy->setPath(poly1->path());
        poly1Copy->offset(numericalAccuracy);// take numerical errors in account
        for(int i = 0; i < poly2->count(); i++){
            if (poly1Copy->containsCoordinate(poly2->vertexCoordinate(i))){
                return false;
            }
        }
        return true;
    } else {
        qWarning("WimaArea::isDisjunct(poly1, poly2):  poly1 == nullptr || poly2 == nullptr!");
        return false;
    }
}

int WimaArea::nextVertexIndex(int index)
{
    if (index >= 0 && index < count()-1) {
        return index + 1;
    } else if (index == count()-1) {
        return 0;
    } else {
        qWarning("WimaArea::nextVertexIndex(): Index out of bounds! index:count = %i:%i", index, count());
        return -1;
    }
}

int WimaArea::previousVertexIndex(int index)
{
    if (index > 0 && index < count()) {
        return index - 1;
    } else if (index == 0) {
        return count()-1;
    } else {
        qWarning("WimaArea::previousVertexIndex(): Index out of bounds! index:count = %i:%i", index, count());
        return -1;
    }
}

bool WimaArea::intersects(QGCMapPolyline *line1, QGCMapPolyline *line2, QGeoCoordinate *intersectionPt)
{
    if (line1 != nullptr && line2 != nullptr && intersectionPt != nullptr) {
        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;
        }


    } else {
        qWarning("WimaArea::intersect(line1, line2): nullptr!");
        return false;
    }
}

bool WimaArea::intersects(QGCMapPolyline *line, WimaArea *poly, QList<QGeoCoordinate> *intersectionList, QList<QPair<int, int> > *neighbourlist)
{
    if (line != nullptr && poly != nullptr && neighbourlist != nullptr && intersectionList != nullptr) {
        neighbourlist->clear();
        intersectionList->clear();

        if (line->count() == 2 && poly->count() >= 3) {

            for (int i = 0; i < poly->count(); i++) {
                QGCMapPolyline polySegment;
                QGeoCoordinate currentVertex = poly->vertexCoordinate(i);
                QGeoCoordinate nextVertex = poly->vertexCoordinate(poly->nextVertexIndex(i));
                polySegment.appendVertex(currentVertex);
                polySegment.appendVertex(nextVertex);

                QGeoCoordinate intersectionPoint;
                //bool retVal2 = intersects(line, line, &intersectionPoint);
                bool retVal = intersects(line, &polySegment, &intersectionPoint);


                if (retVal != false){
                    intersectionList->append(intersectionPoint);

                    QPair<int, int>     neighbours;
                    neighbours.first    = i;
                    neighbours.second   = poly->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;
        }

    } else {
        qWarning("WimaArea::intersects(line, poly): nullptr!");
        return false;
    }
}

double WimaArea::distInsidePoly(QGeoCoordinate *c1, QGeoCoordinate *c2, WimaArea *poly)
{
    if (c1 != nullptr && c2 != nullptr && poly != nullptr) {
        // conditions which must be fullfilled
        bool c1InPolyRim   = poly->coordinateList().contains(*c1);
        bool c2InPolyRim   = poly->coordinateList().contains(*c2);
        bool c1InsidePoly  = poly->containsCoordinate(*c1);
        bool c2InsidePoly  = poly->containsCoordinate(*c2);

        WimaArea bigPoly = *poly;
        bigPoly.offset(0.1);
        if ( (c1InPolyRim || c1InsidePoly) && (c2InPolyRim || c2InsidePoly)) {
            QList<QGeoCoordinate>   intersectionList;
            QList<QPair<int, int>>  neighbourlist;
            QGCMapPolyline line;

            line.appendVertex(*c1);
            line.appendVertex(*c2);
            intersects(&line, &bigPoly, &intersectionList, &neighbourlist);

            //if ( intersectionList.size() == (c1InPolyRim || c2InPolyRim ? 2:0) ){
            if ( intersectionList.size() == 0 ){
                return c1->distanceTo(*c2);
            }

        }
        return std::numeric_limits<qreal>::infinity();
    } else {
        qWarning("WimaArea::distInsidePoly(QGeoCoordinate*, QGeoCoordinate*, WimaArea*): nullptr!");
        return std::numeric_limits<qreal>::infinity();
    }
}

void WimaArea::dijkstraPath(QGeoCoordinate *start, QGeoCoordinate *end, WimaArea *poly, QList<QGeoCoordinate>* dijkistraPath)
{
    if (start != nullptr && end != nullptr && poly != nullptr && dijkistraPath != nullptr) {
        struct Node{
            QGeoCoordinate coordinate;
            double distance = std::numeric_limits<qreal>::infinity();
            Node* predecessorNode = nullptr;
        };

        QList<Node> nodeList;
        QList<Node*> workingSet;


        // initialize
        // start
        Node startNode;
        startNode.coordinate    = *start;
        startNode.distance      = 0;
        nodeList.append(startNode);

        //poly
        for (int i = 0; i < poly->count(); i++) {
            Node node;
            node.coordinate = poly->vertexCoordinate(i);
            nodeList.append(node);
        }

        //end
        Node endNode;
        endNode.coordinate  = *end;
        nodeList.append(endNode);

        // 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?
                double dist = distInsidePoly(&u->coordinate, &v->coordinate, poly);
                double alternative = u->distance + dist;
                if (alternative < v->distance)  {
                    v->distance         = alternative;
                    v->predecessorNode  = u;
                }
            }

        }

        // create path
        Node* Node = &nodeList.last();
        if (Node->predecessorNode == nullptr) {
            qWarning("WimaArea::dijkstraPath(): Error, no path found!");
            return;
        }

        while (1) {
            dijkistraPath->prepend(Node->coordinate);

            //Update Node
            Node = Node->predecessorNode;
            if (Node == nullptr) {
                break;
            }
        }


    } else {
        qWarning(" WimaArea::dijkstraPath(QGeoCoordinate*, QGeoCoordinate*, WimaArea*): nullptr!");
        return;
    }
}