WimaArea.cc

#include "WimaArea.h"

const double WimaArea::numericalAccuracy    = 1e-3; // meters
const char* WimaArea::maxAltitudeName       = "maxAltitude";
const char* WimaArea::wimaAreaName          = "WimaArea";
const char* WimaArea::areaTypeName          = "AreaType";




WimaArea::WimaArea(QObject *parent)
    :  QGCMapPolygon (parent)
{
    init();
    _maxAltitude = 30;
}

WimaArea::WimaArea(const WimaArea &other, QObject *parent)
    : QGCMapPolygon (other, parent)
{
    init();
    this->setPath(other.path());
    this->setCenter(other.center());
    this->setCenterDrag(other.centerDrag());
    this->setInteractive(other.interactive());
    _maxAltitude = other.maxAltitude();
}

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

int WimaArea::getClosestVertexIndex(const QGeoCoordinate &coordinate) const
{
    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(const QGeoCoordinate& coordinate) const
{
    return this->vertexCoordinate(getClosestVertexIndex(coordinate));
}

QGCMapPolygon WimaArea::toQGCPolygon(const WimaArea &poly)
{
    QGCMapPolygon qgcPoly;
    qgcPoly.setPath(poly.path());
    qgcPoly.setCenter(poly.center());
    qgcPoly.setCenterDrag(poly.centerDrag());
    qgcPoly.setInteractive(poly.interactive());

    return QGCMapPolygon(qgcPoly);
}

QGCMapPolygon WimaArea::toQGCPolygon() const
{
    return toQGCPolygon(*this);
}

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

bool WimaArea::join(WimaArea &poly1, WimaArea &poly2, WimaArea &joinedPoly)
{

        if (poly1.count() >= 3 && poly2.count() >= 3) {

            joinedPoly.clear();

            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 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);
                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) {
                    if (poly1.count() == joinedPoly.count()) { // is the case if poly1 and poly2 don't intersect
                        return false;
                    } else {
                        return true;
                    }
                }
            }

        } else {
            return false;
        }
}

bool WimaArea::join(WimaArea &poly)
{
    WimaArea joinedArea;
    if ( join(*this, poly, joinedArea) ) {
        this->setPath(joinedArea.path());
        return true;
    } else {
        return false;
    }
}

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) const
{
    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) const
{
    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(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;
        }
}

bool WimaArea::intersects(const QGCMapPolyline      &line,
                          const WimaArea            &poly,
                          QList<QGeoCoordinate>     &intersectionList,
                          QList<QPair<int, int> >   &neighbourList)
{
       // ================ Brief Explanation ================
       // This function checks whether line intersects with the border line of poly
       // Returns true if at least one intersection occurs, false else.
       // Stores the intersection points inside intersectionList.
       // Stores the indices of the cloest two polygon vetices for each of coorespoinding intersection points in neighbourList
        intersectionList.clear();
        neighbourList.clear();


        if (line.count() == 2 && poly.count() >= 3) { // are line a proper line and poly a proper poly?

            // Asseble a line form each tow consecutive polygon vertices and check whether it intersects with line
            for (int i = 0; i < poly.count(); i++) {

                QGCMapPolyline interatorLine;
                QGeoCoordinate currentVertex    = poly.vertexCoordinate(i);
                QGeoCoordinate nextVertex       = poly.vertexCoordinate(poly.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   = 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;
        }
}

double WimaArea::distInsidePoly(const QGeoCoordinate &c1, const QGeoCoordinate &c2, WimaArea poly)
{
        poly.offset(0.1); // hack to compensate for numerical issues, migh be replaced in the future...
        if ( poly.containsCoordinate(c1) && poly.containsCoordinate(c2)) {
            QList<QGeoCoordinate>   intersectionList;
            QList<QPair<int, int>>  neighbourlist;
            QGCMapPolyline line;

            line.appendVertex(c1);
            line.appendVertex(c2);
            intersects(line, poly, intersectionList, neighbourlist);

            if ( intersectionList.size() == 0 ){
                return c1.distanceTo(c2);
            } else {
                return std::numeric_limits<qreal>::infinity();
            }

        } else {
            return std::numeric_limits<qreal>::infinity();
        }
}

bool WimaArea::dijkstraPath(const QGeoCoordinate    &start,
                            const QGeoCoordinate    &end,
                            const WimaArea          &poly,
                            QList<QGeoCoordinate>   &dijkstraPath)
{
    // ================ Brief Explanation ================
    // This function calculates the shortes Path between two GeoCoordinates (start, end) using the Dijkstra Algorithm.
    // The path will be inside the polygon poly if possible.
    // Stores the result inside dijkstraPath
    // Returns true if a valid path was found.
    if ( isSelfIntersecting(poly) ) {
        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
    // start cooridnate
    Node startNode;
    startNode.coordinate    = start;
    startNode.distance      = 0;
    nodeList.append(startNode);

    //poly cooridnates
    for (int i = 0; i < poly.count(); i++) {
        Node node;
        node.coordinate = poly.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, poly);
            // 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 sucessful
    Node* Node = &nodeList.last();
    if (Node->predecessorNode == nullptr) {
        qWarning("WimaArea::dijkstraPath(): Error, no path found!");
        return false;
    }

    // assemble path
    while (1) {
        dijkstraPath.prepend(Node->coordinate);

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

    return true;
}

bool WimaArea::isSelfIntersecting(const WimaArea &poly)
{
    int i = 0;
    if (poly.count() > 3) {
        while(i < poly.count()-1) {
            QGCMapPolyline refLine;
            refLine.appendVertex(poly.vertexCoordinate(i));
            refLine.appendVertex(poly.vertexCoordinate(poly.nextVertexIndex(i)));
            int j = poly.nextVertexIndex(i);
            while(j < poly.count()) {
                QGeoCoordinate dummy;
                QGCMapPolyline iteratorLine;
                iteratorLine.appendVertex(poly.vertexCoordinate(j));
                iteratorLine.appendVertex(poly.vertexCoordinate(poly.nextVertexIndex(j)));

                if ( intersects(refLine, iteratorLine, dummy) )
                    return true;

                j++;
            }
            i++;
        }
    }

    return false;
}

bool WimaArea::isSelfIntersecting()
{
    return isSelfIntersecting(*this);
}

void WimaArea::saveToJson(QJsonObject &json)
{
    this->QGCMapPolygon::saveToJson(json);
    json[maxAltitudeName]   = _maxAltitude;
    json[areaTypeName]      = wimaAreaName;
    // add WimaVehicle if necessary..
}

bool WimaArea::loadFromJson(const QJsonObject &json, QString& errorString)
{
    if ( this->QGCMapPolygon::loadFromJson(json, false /*no poly required*/, errorString) ) {
        if ( json.contains(maxAltitudeName) && json[maxAltitudeName].isDouble()) {
            _maxAltitude = json[maxAltitudeName].toDouble();
            return true;
        } else {
            errorString.append(tr("Could not load Maximum Altitude value!\n"));
            return false;
        }
    } else {
        qWarning() << errorString;
        return false;
    }
}

void WimaArea::update(const WimaArea &area)
{
    this->QGCMapPolygon::update(area);
    this->setMaxAltitude(area.maxAltitude());
}

void WimaArea::init()
{
    this->setObjectName(wimaAreaName);
}

void print(const WimaArea &area)
{
    QString message;
    print(area, message);
    qWarning() << message;
}

void print(const WimaArea &area, QString &outputString)
{
    outputString.append(QString("Type: %1").arg(area.objectName()));
    print(static_cast<const QGCMapPolygon&>(area), outputString);
    outputString.append(QString("Maximum Altitude: %1").arg(area._maxAltitude));
}