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()
    : QGCMapPolygon (nullptr)
{
    this->setObjectName(wimaAreaName);
    _maxAltitude = 30;
    _wimaVehicle = new WimaVehicle(this);
}

WimaArea::WimaArea(QObject *parent)
    :   QGCMapPolygon (parent)
{
    this->setObjectName(wimaAreaName);
    _maxAltitude = 30;
    _wimaVehicle = new WimaVehicle(this);
}

WimaArea::WimaArea(const QGCMapPolygon &other, QObject *parent)
    : QGCMapPolygon (other, parent)
{
    this->setObjectName(wimaAreaName);
    _maxAltitude = 30;
    _wimaVehicle = new WimaVehicle(this);
}

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


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(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) const
{
    QGCMapPolygon qgcPoly;
    qgcPoly.setPath(poly.path());
    qgcPoly.setCenter(poly.center());
    qgcPoly.setCenterDrag(poly.centerDrag());
    qgcPoly.setInteractive(poly.interactive());

    return QGCMapPolygon(qgcPoly);
}

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

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

        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;
        }
}

void WimaArea::join(WimaArea &poly)
{
    WimaArea joinedArea;
    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) 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)
{
        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;
        }
}

double WimaArea::distInsidePoly(const QGeoCoordinate &c1, const QGeoCoordinate &c2, const WimaArea &poly)
{
        WimaArea bigPoly(poly);
        bigPoly.offset(0.01); // hack to compensate for numerical issues, migh be replaced in the future...
        if ( bigPoly.containsCoordinate(c1) && bigPoly.containsCoordinate(c2)) {
            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);
            } else {
                return std::numeric_limits<qreal>::infinity();
            }

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

void WimaArea::dijkstraPath(const QGeoCoordinate &start, const QGeoCoordinate &end, const WimaArea &poly, QList<QGeoCoordinate> &dijkstraPath)
{
    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) {
        dijkstraPath.prepend(Node->coordinate);

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

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("Could not load Maximum Altitude value!\n");
            return false;
        }
    } else {
        qWarning() << errorString;
        return false;
    }
}