123

src/Wima/CS_old.cc

+#include "CircularSurveyComplexItem.h"
+#include "JsonHelper.h"
+#include "QGCApplication.h"
+#include <chrono>
+
+
+const char* CircularSurveyComplexItem::settingsGroup =              "CircularSurvey";
+const char* CircularSurveyComplexItem::deltaRName =                 "DeltaR";
+const char* CircularSurveyComplexItem::deltaAlphaName =             "DeltaAlpha";
+const char* CircularSurveyComplexItem::transectMinLengthName =      "TransectMinLength";
+const char* CircularSurveyComplexItem::fixedDirectionName =         "FixedDirection";
+const char* CircularSurveyComplexItem::reverseName =                "Reverse";
+const char* CircularSurveyComplexItem::maxWaypointsName =           "MaxWaypoints";
+
+
+const char* CircularSurveyComplexItem::jsonComplexItemTypeValue     =   "circularSurvey";
+const char* CircularSurveyComplexItem::jsonDeltaRKey                =   "deltaR";
+const char* CircularSurveyComplexItem::jsonDeltaAlphaKey            =   "deltaAlpha";
+const char* CircularSurveyComplexItem::jsonTransectMinLengthKey     =   "transectMinLength";
+const char* CircularSurveyComplexItem::jsonfixedDirectionKey        =   "fixedDirection";
+const char* CircularSurveyComplexItem::jsonReverseKey               =   "reverse";
+const char* CircularSurveyComplexItem::jsonReferencePointLatKey     =   "referencePointLat";
+const char* CircularSurveyComplexItem::jsonReferencePointLongKey    =   "referencePointLong";
+const char* CircularSurveyComplexItem::jsonReferencePointAltKey     =   "referencePointAlt";
+
+CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile, QObject *parent)
+    :   TransectStyleComplexItem    (vehicle, flyView, settingsGroup, parent)
+    ,   _referencePoint             (QGeoCoordinate(0, 0,0))
+    ,   _metaDataMap                (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this))
+    ,   _deltaR                     (settingsGroup, _metaDataMap[deltaRName])
+    ,   _deltaAlpha                 (settingsGroup, _metaDataMap[deltaAlphaName])
+    ,   _transectMinLength          (settingsGroup, _metaDataMap[transectMinLengthName])
+    ,   _fixedDirection             (settingsGroup, _metaDataMap[fixedDirectionName])
+    ,   _reverse                    (settingsGroup, _metaDataMap[reverseName])
+    ,   _maxWaypoints               (settingsGroup, _metaDataMap[maxWaypointsName])
+    ,   _isInitialized              (false)
+    ,   _reverseOnly                (false)
+{
+    Q_UNUSED(kmlOrShpFile)
+    _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";
+    connect(&_deltaR,               &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc);
+    connect(&_deltaAlpha,           &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc);
+    connect(&_transectMinLength,    &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc);
+    connect(&_fixedDirection,       &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc);
+    connect(&_maxWaypoints,         &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc);
+    connect(&_reverse,              &Fact::valueChanged, this, &CircularSurveyComplexItem::_reverseTransects);
+    connect(this,                   &CircularSurveyComplexItem::refPointChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc);
+    //connect(&_cameraCalc.distanceToSurface(), &Fact::rawValueChanged, this->)
+
+}
+
+void CircularSurveyComplexItem::resetReference()
+{
+    setRefPoint(_surveyAreaPolygon.center());
+}
+
+void CircularSurveyComplexItem::comprehensiveUpdate()
+{
+    _triggerSlowRecalc();
+}
+
+void CircularSurveyComplexItem::setRefPoint(const QGeoCoordinate &refPt)
+{
+    if (refPt != _referencePoint){
+        _referencePoint = refPt;
+
+        emit refPointChanged();
+    }
+}
+
+void CircularSurveyComplexItem::setIsInitialized(bool isInitialized)
+{
+    if (isInitialized != _isInitialized) {
+        _isInitialized = isInitialized;
+
+        emit isInitializedChanged();
+    }
+}
+
+QGeoCoordinate CircularSurveyComplexItem::refPoint() const
+{
+    return _referencePoint;
+}
+
+Fact *CircularSurveyComplexItem::deltaR()
+{
+    return &_deltaR;
+}
+
+Fact *CircularSurveyComplexItem::deltaAlpha()
+{
+    return &_deltaAlpha;
+}
+
+bool CircularSurveyComplexItem::isInitialized()
+{
+    return _isInitialized;
+}
+
+bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString)
+{
+    // We need to pull version first to determine what validation/conversion needs to be performed
+    QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
+        { JsonHelper::jsonVersionKey, QJsonValue::Double, true },
+    };
+    if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList, errorString)) {
+        return false;
+    }
+
+    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
+    if (version != 1) {
+        errorString = tr("Survey items do not support version %1").arg(version);
+        return false;
+    }
+
+    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
+        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
+        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
+        { jsonDeltaRKey,                                QJsonValue::Double, true },
+        { jsonDeltaAlphaKey,                            QJsonValue::Double, true },
+        { jsonTransectMinLengthKey,                     QJsonValue::Double, true },
+        { jsonfixedDirectionKey,                        QJsonValue::Bool,   true },
+        { jsonReverseKey,                               QJsonValue::Bool,   true },
+        { jsonReferencePointLatKey,                     QJsonValue::Double, true },
+        { jsonReferencePointLongKey,                    QJsonValue::Double, true },
+        { jsonReferencePointAltKey,                     QJsonValue::Double, true },
+    };
+
+    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
+        return false;
+    }
+
+    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
+    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
+    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
+        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
+        return false;
+    }
+
+    _ignoreRecalc = true;
+
+    setSequenceNumber(sequenceNumber);
+
+    if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) {
+        _surveyAreaPolygon.clear();
+        return false;
+    }
+
+    if (!_load(complexObject, errorString)) {
+        _ignoreRecalc = false;
+        return false;
+    }
+
+    _deltaR.setRawValue             (complexObject[jsonDeltaRKey].toDouble());
+    _deltaAlpha.setRawValue         (complexObject[jsonDeltaAlphaKey].toDouble());
+    _transectMinLength.setRawValue  (complexObject[jsonTransectMinLengthKey].toDouble());
+    _referencePoint.setLongitude    (complexObject[jsonReferencePointLongKey].toDouble());
+    _referencePoint.setLatitude     (complexObject[jsonReferencePointLatKey].toDouble());
+    _referencePoint.setAltitude     (complexObject[jsonReferencePointAltKey].toDouble());
+    _fixedDirection.setRawValue     (complexObject[jsonfixedDirectionKey].toBool());
+    _reverse.setRawValue            (complexObject[jsonReverseKey].toBool());
+    setIsInitialized(true);
+
+    _ignoreRecalc = false;
+
+    _recalcComplexDistance();
+    if (_cameraShots == 0) {
+        // Shot count was possibly not available from plan file
+        _recalcCameraShots();
+    }
+
+    return true;
+}
+
+void CircularSurveyComplexItem::save(QJsonArray &planItems)
+{
+    QJsonObject saveObject;
+
+    _save(saveObject);
+
+    saveObject[JsonHelper::jsonVersionKey] =                    1;
+    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
+    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
+
+    saveObject[jsonDeltaRKey]               = _deltaR.rawValue().toDouble();
+    saveObject[jsonDeltaAlphaKey]           = _deltaAlpha.rawValue().toDouble();    
+    saveObject[jsonTransectMinLengthKey]    = _transectMinLength.rawValue().toDouble();
+    saveObject[jsonfixedDirectionKey]       = _fixedDirection.rawValue().toBool();
+    saveObject[jsonReverseKey]              = _reverse.rawValue().toBool();
+    saveObject[jsonReferencePointLongKey]   = _referencePoint.longitude();
+    saveObject[jsonReferencePointLatKey]    = _referencePoint.latitude();
+    saveObject[jsonReferencePointAltKey]    = _referencePoint.altitude();
+
+    // Polygon shape
+    _surveyAreaPolygon.saveToJson(saveObject);
+
+    planItems.append(saveObject);
+}
+
+void CircularSurveyComplexItem::appendMissionItems(QList<MissionItem *> &items, QObject *missionItemParent)
+{
+    if (_transectsDirty)
+        return;
+    if (_loadedMissionItems.count()) {
+        // We have mission items from the loaded plan, use those
+        _appendLoadedMissionItems(items, missionItemParent);
+    } else {
+        // Build the mission items on the fly
+        _buildAndAppendMissionItems(items, missionItemParent);
+    }
+}
+
+void CircularSurveyComplexItem::_appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
+{
+    //qCDebug(SurveyComplexItemLog) << "_appendLoadedMissionItems";
+    if (_transectsDirty)
+        return;
+    int seqNum = _sequenceNumber;
+
+    for (const MissionItem* loadedMissionItem: _loadedMissionItems) {
+        MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent);
+        item->setSequenceNumber(seqNum++);
+        items.append(item);
+    }
+}
+
+void CircularSurveyComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
+{
+    // original code: SurveyComplexItem::_buildAndAppendMissionItems()
+    //qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems";
+
+    // Now build the mission items from the transect points
+
+    if (_transectsDirty)
+        return;
+
+    MissionItem* item;
+    int seqNum =                    _sequenceNumber;
+
+    MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative()
+            ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;
+
+    for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect : _transects) {
+        //bool transectEntry = true;
+
+        for (const CoordInfo_t& transectCoordInfo: transect) {
+            item = new MissionItem(seqNum++,
+                                   MAV_CMD_NAV_WAYPOINT,
+                                   mavFrame,
+                                   0,                                           // Hold time (delay for hover and capture to settle vehicle before image is taken)
+                                   0.0,                                         // No acceptance radius specified
+                                   0.0,                                         // Pass through waypoint
+                                   std::numeric_limits<double>::quiet_NaN(),    // Yaw unchanged
+                                   transectCoordInfo.coord.latitude(),
+                                   transectCoordInfo.coord.longitude(),
+                                   transectCoordInfo.coord.altitude(),
+                                   true,                                        // autoContinue
+                                   false,                                       // isCurrentItem
+                                   missionItemParent);
+            items.append(item);
+        }
+    }
+}
+
+void CircularSurveyComplexItem::applyNewAltitude(double newAltitude)
+{
+    _cameraCalc.valueSetIsDistance()->setRawValue(true);
+    _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
+    _cameraCalc.setDistanceToSurfaceRelative(true);
+}
+
+double CircularSurveyComplexItem::timeBetweenShots()
+{
+    return 1;
+}
+
+bool CircularSurveyComplexItem::readyForSave() const
+{
+    return TransectStyleComplexItem::readyForSave();
+}
+
+double CircularSurveyComplexItem::additionalTimeDelay() const
+{
+    return 0;
+}
+void CircularSurveyComplexItem::_rebuildTransectsPhase1(void){
+    if (_doFastRecalc){
+        _rebuildTransectsFast();
+    } else {
+        _rebuildTransectsSlow();
+        _doFastRecalc = true;
+    }
+}
+
+void CircularSurveyComplexItem::_rebuildTransectsFast()
+{
+    using namespace GeoUtilities;
+    using namespace PolygonCalculus;
+    using namespace PlanimetryCalculus;
+
+    _transects.clear();
+
+    QPolygonF surveyPolygon;
+    toCartesianList(_surveyAreaPolygon.coordinateList(), _referencePoint, surveyPolygon);
+    if (!_rebuildTransectsInputCheck(surveyPolygon))
+        return;
+
+    // If the transects are getting rebuilt then any previously loaded mission items are now invalid
+    if (_loadedMissionItemsParent) {
+        _loadedMissionItems.clear();
+        _loadedMissionItemsParent->deleteLater();
+        _loadedMissionItemsParent = nullptr;
+    }
+
+    QVector<QVector<QPointF>> transectPath;
+    if(!_generateTransectPath(transectPath, surveyPolygon))
+        return;
+
+    /// optimize path to snake or zig-zag pattern
+    bool fixedDirectionBool = _fixedDirection.rawValue().toBool();
+    QVector<QPointF> currentSection = transectPath.takeFirst();
+    if ( currentSection.isEmpty() )
+        return;
+
+    QVector<QPointF> optiPath; // optimized path
+    while( !transectPath.empty() ) {
+        optiPath.append(currentSection);
+        QPointF endVertex = currentSection.last();
+        double minDist = std::numeric_limits<double>::infinity();
+        int index = 0;
+        bool reversePath = false;
+
+        // iterate over all paths in fullPath and assign the one with the shortest distance to endVertex to currentSection
+        for (int i = 0; i < transectPath.size(); i++) {
+            auto iteratorPath = transectPath[i];
+            double dist = PlanimetryCalculus::distance(endVertex, iteratorPath.first());
+            if ( dist < minDist ) {
+                minDist = dist;
+                index = i;
+                reversePath = false;
+            }
+            dist = PlanimetryCalculus::distance(endVertex, iteratorPath.last());
+            if (dist < minDist) {
+                minDist = dist;
+                index = i;
+                reversePath = true;
+            }
+        }
+        currentSection = transectPath.takeAt(index);
+        if (reversePath && !fixedDirectionBool) {
+            PolygonCalculus::reversePath(currentSection);
+        }
+    }
+
+    optiPath.append(currentSection); // append last section
+
+    if (optiPath.size() > _maxWaypoints.rawValue().toInt())
+        return;
+
+
+    _rebuildTransectsToGeo(optiPath, _referencePoint);
+    _transectsDirty = true;
+}
+
+
+void CircularSurveyComplexItem::_rebuildTransectsSlow()
+{
+    using namespace GeoUtilities;
+    using namespace PolygonCalculus;
+    using namespace PlanimetryCalculus;
+
+    if (_reverseOnly) { // reverse transects and return
+        _reverseOnly = false;
+
+        QList<QList<CoordInfo_t>>  transectsReverse;
+        transectsReverse.reserve(_transects.size());
+        for (auto list : _transects) {
+            QList<CoordInfo_t> listReverse;
+            for (auto coordinate : list)
+                    listReverse.prepend(coordinate);
+
+            transectsReverse.prepend(listReverse);
+        }
+        _transects = transectsReverse;
+
+        return;
+    }
+
+    _transects.clear();
+
+    QPolygonF surveyPolygon;
+    toCartesianList(_surveyAreaPolygon.coordinateList(), _referencePoint, surveyPolygon);
+    if (!_rebuildTransectsInputCheck(surveyPolygon))
+        return;
+
+    // If the transects are getting rebuilt then any previously loaded mission items are now invalid.
+    if (_loadedMissionItemsParent) {
+        _loadedMissionItems.clear();
+        _loadedMissionItemsParent->deleteLater();
+        _loadedMissionItemsParent = nullptr;
+    }
+
+    QVector<QVector<QPointF>> transectPath;
+    if(!_generateTransectPath(transectPath, surveyPolygon))
+        return;
+
+    // optimize path to snake or zig-zag pattern
+    const bool fixedDirectionBool = _fixedDirection.rawValue().toBool();
+    QVector<QPointF> currentSection = transectPath.takeFirst(); if ( currentSection.isEmpty() ) return;
+    QVector<QPointF> optimizedPath(currentSection);
+    bool reversePath = true; // controlls if currentSection gets reversed, has nothing todo with _reverseOnly
+    while( !transectPath.empty() ) {
+        QPointF endVertex = currentSection.last();
+        double minDist = std::numeric_limits<double>::infinity();
+        int index = 0;
+
+        // iterate over all paths in fullPath and assign the one with the shortest distance to endVertex to currentSection
+        QVector<QPointF> connectorPath;
+        for (int i = 0; i < transectPath.size(); i++) {
+            QVector<QPointF> iteratorPath = transectPath[i];
+            QVector<QPointF> tempConnectorPath;
+            bool retVal;
+
+            if (reversePath  && !fixedDirectionBool) {
+                 retVal = PolygonCalculus::shortestPath(surveyPolygon, endVertex, iteratorPath.last(), tempConnectorPath);
+            } else {
+                 retVal = PolygonCalculus::shortestPath(surveyPolygon, endVertex, iteratorPath.first(), tempConnectorPath);
+            }
+
+            if (!retVal)
+                qWarning("CircularSurveyComplexItem::_rebuildTransectsPhase1: internal error; false shortestPath");
+
+            double dist = 0;
+            for (int i = 0; i < tempConnectorPath.size()-1; ++i)
+                dist += PlanimetryCalculus::distance(tempConnectorPath[i], tempConnectorPath[i+1]);
+
+            if (dist < minDist) {
+                minDist = dist;
+                index = i;
+                connectorPath = tempConnectorPath;
+            }
+        }
+        currentSection = transectPath.takeAt(index);
+        if (reversePath && !fixedDirectionBool) {
+            PolygonCalculus::reversePath(currentSection);
+        }
+
+        reversePath ^= true; // toggle
+
+        connectorPath.pop_front();
+        connectorPath.pop_back();
+        if (connectorPath.size() > 0)
+            optimizedPath.append(connectorPath);
+        optimizedPath.append(currentSection);
+    }
+    if (optimizedPath.size() > _maxWaypoints.rawValue().toInt())
+        return;
+
+    _rebuildTransectsToGeo(optimizedPath, _referencePoint);
+
+    _transectsDirty      = false;
+    //_triggerSlowRecalcTimer.stop(); // stop any pending slow recalculations
+    return;
+}
+
+void CircularSurveyComplexItem::_triggerSlowRecalc()
+{
+    _doFastRecalc = false;
+    _rebuildTransects();
+}
+
+void CircularSurveyComplexItem::_recalcComplexDistance()
+{
+    _complexDistance = 0;
+    if (_transectsDirty)
+        return;
+    for (int i=0; i<_visualTransectPoints.count() - 1; i++) {
+        _complexDistance += _visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(_visualTransectPoints[i+1].value<QGeoCoordinate>());
+    }
+    emit complexDistanceChanged();
+}
+
+// no cameraShots in Circular Survey, add if desired
+void CircularSurveyComplexItem::_recalcCameraShots()
+{
+    _cameraShots = 0;
+}
+
+void CircularSurveyComplexItem::_reverseTransects()
+{
+    _reverseOnly = true;
+    _triggerSlowRecalc();
+}
+
+bool CircularSurveyComplexItem::_shortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &shortestPath)
+{
+    using namespace GeoUtilities;
+    using namespace PolygonCalculus;
+    QPolygonF polygon2D;
+    toCartesianList(this->surveyAreaPolygon()->coordinateList(), /*origin*/ start, polygon2D);
+    QPointF start2D(0,0);
+    QPointF end2D;
+    toCartesian(destination, start, end2D);
+    QVector<QPointF> path2D;
+
+    bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
+    if (retVal)
+        toGeoList(path2D, /*origin*/ start, shortestPath);
+
+    return  retVal;
+}
+
+bool CircularSurveyComplexItem::_generateTransectPath(QVector<QVector<QPointF> > &transectPath, const QPolygonF &surveyPolygon)
+{
+    using namespace PlanimetryCalculus;
+    QVector<double> distances;
+    for (const QPointF &p : surveyPolygon) distances.append(norm(p));
+
+    // fetch input data
+    double dalpha   = _deltaAlpha.rawValue().toDouble()/180.0*M_PI; // angle discretisation of circles
+    double dr       = _deltaR.rawValue().toDouble(); // distance between circles
+    double lmin     = _transectMinLength.rawValue().toDouble(); // minimal transect length
+    double r_min    = dr; // minimal circle radius
+    double r_max    = (*std::max_element(distances.begin(), distances.end())); // maximal circle radius
+    unsigned int maxWaypoints = _maxWaypoints.rawValue().toUInt();
+
+    QPointF origin(0, 0);
+    IntersectType type;
+    bool originInside = true;
+    if (!contains(surveyPolygon, origin, type)) {
+        QVector<double> angles;
+        for (const QPointF &p : surveyPolygon) angles.append(truncateAngle(angle(p)));
+
+        // determine r_min by successive approximation
+        double r = r_min;
+        while ( r < r_max) {
+            Circle circle(r, origin);
+
+            if (intersects(circle, surveyPolygon)) {
+                r_min = r;
+                break;
+            }
+
+            r += dr;
+        }
+        originInside = false;
+    }
+
+    double r = r_min;
+
+    unsigned int waypointCounter = 0;
+    while (r < r_max) {
+        Circle circle(r, origin);
+        QVector<QPointFVector> intersectPoints;
+        QVector<IntersectType> typeList;
+        QVector<QPair<int, int>> neighbourList;
+        if (intersects(circle, surveyPolygon, intersectPoints, neighbourList, typeList)) {
+
+            // intersection Points between circle and polygon, entering polygon
+            // when walking in counterclockwise direction along circle
+            QVector<QPointF> entryPoints;
+            // intersection Points between circle and polygon, leaving polygon
+            // when walking in counterclockwise direction along circle
+            QVector<QPointF> exitPoints;
+            // determine entryPoints and exit Points
+            for (int j = 0; j < intersectPoints.size(); j++) {
+                QVector<QPointF> intersects = intersectPoints[j]; // one pt = tangent, two pt = sekant
+
+                QPointF p1 = surveyPolygon[neighbourList[j].first];
+                QPointF p2 = surveyPolygon[neighbourList[j].second];
+                QLineF intersetLine(p1, p2);
+                double lineAngle = angle(intersetLine);
+
+                for (QPointF ipt : intersects) {
+                    double circleTangentAngle = angle(ipt)+M_PI_2;
+                    if (   !qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle))
+                        && !qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle - M_PI) ))
+                        {
+                        if (truncateAngle(circleTangentAngle - lineAngle)  > M_PI) {
+                            entryPoints.append(ipt);
+                        } else {
+                            exitPoints.append(ipt);
+                        }
+                    }
+                }
+            }
+
+            // sort
+            std::sort(entryPoints.begin(), entryPoints.end(), [](QPointF p1, QPointF p2) {
+               return angle(p1) < angle(p2);
+            });
+            std::sort(exitPoints.begin(), exitPoints.end(), [](QPointF p1, QPointF p2) {
+               return angle(p1) < angle(p2);
+            });
+
+            // match entry and exit points
+            int offset = 0;
+            double minAngle = std::numeric_limits<double>::infinity();
+            for (int k = 0; k < exitPoints.size(); k++) {
+                QPointF pt = exitPoints[k];
+                double alpha = truncateAngle(angle(pt) - angle(entryPoints[0]));
+                if (minAngle > alpha) {
+                    minAngle = alpha;
+                    offset = k;
+                }
+            }
+
+            // generate circle sectors
+            for (int k = 0; k < entryPoints.size(); k++) {
+                double alpha1 = angle(entryPoints[k]);
+                double alpha2 = angle(exitPoints[(k+offset) % entryPoints.size()]);
+                double dAlpha = truncateAngle(alpha2-alpha1);
+                int numNodes = int(ceil(dAlpha/dalpha)) + 1;
+
+                QVector<QPointF> sectorPath = circle.approximateSektor(numNodes, alpha1, alpha2);
+                // use shortestPath() here if necessary, could be a problem if dr >>
+                if (sectorPath.size() > 0) {
+                    waypointCounter += uint(sectorPath.size());
+                    if (waypointCounter > maxWaypoints )
+                        return false;
+                    transectPath.append(sectorPath);
+                }
+            }
+        } else if (originInside) {
+            // circle fully inside polygon
+            int numNodes = int(ceil(2*M_PI/dalpha)) + 1;
+            QVector<QPointF> sectorPath = circle.approximateSektor(numNodes, 0, 2*M_PI);
+            // use shortestPath() here if necessary, could be a problem if dr >>
+            waypointCounter += uint(sectorPath.size());
+            if (waypointCounter > maxWaypoints )
+                return false;
+            transectPath.append(sectorPath);
+        }
+        r += dr;
+     }
+
+    if (transectPath.size() == 0)
+        return false;;
+
+    // remove short transects
+    for (int i = 0; i < transectPath.size(); i++) {
+        auto transect = transectPath[i];
+        double len = 0;
+        for (int j = 0; j < transect.size()-1; ++j) {
+            len += PlanimetryCalculus::distance(transect[j], transect[j+1]);
+        }
+
+        if (len < lmin)
+            transectPath.removeAt(i--);
+    }
+    if (transectPath.size() == 0)
+        return false;
+
+    return true;
+}
+
+bool CircularSurveyComplexItem::_rebuildTransectsInputCheck(QPolygonF &poly)
+{
+    // rebuild not necessary?
+    if (!_isInitialized)
+        return false;
+
+    // check if input is valid
+    if ( _surveyAreaPolygon.count() < 3)
+        return false;
+
+    // some more checks
+    if (!PolygonCalculus::isSimplePolygon(poly))
+        return false;
+
+    // even more checks
+    if (!PolygonCalculus::hasClockwiseWinding(poly))
+        PolygonCalculus::reversePath(poly);
+
+    // check if input is valid
+    if (   _deltaAlpha.rawValue() > _deltaAlpha.rawMax()
+           && _deltaAlpha.rawValue() < _deltaAlpha.rawMin())
+        return false;
+    if (   _deltaR.rawValue() > _deltaR.rawMax()
+           && _deltaR.rawValue() < _deltaR.rawMin())
+        return false;
+
+    return true;
+}
+
+void CircularSurveyComplexItem::_rebuildTransectsToGeo(const QVector<QPointF> &path, const QGeoCoordinate &reference)
+{
+    using namespace GeoUtilities;
+
+    QVector<QGeoCoordinate> geoPath;
+    toGeoList(path, reference, geoPath);
+    QList<CoordInfo_t> transectList;
+    transectList.reserve(path.size());
+    for ( const QGeoCoordinate &coordinate : geoPath) {
+        CoordInfo_t coordinfo = {coordinate, CoordTypeInterior};
+        transectList.append(coordinfo);
+    }
+    _transects.append(transectList);
+}
+
+
+Fact *CircularSurveyComplexItem::transectMinLength()
+{
+    return &_transectMinLength;
+}
+
+Fact *CircularSurveyComplexItem::fixedDirection()
+{
+    return &_fixedDirection;
+}
+
+Fact *CircularSurveyComplexItem::reverse()
+{
+    return &_reverse;
+}
+
+Fact *CircularSurveyComplexItem::maxWaypoints()
+{
+    return &_maxWaypoints;
+}
+
+
+
+
+/*!
+    \class CircularSurveyComplexItem
+    \inmodule Wima
+
+    \brief The \c CircularSurveyComplexItem class provides a survey mission item with circular transects around a point of interest.
+
+    CircularSurveyComplexItem class provides a survey mission item with circular transects around a point of interest. Within the
+    \c Wima module it's used to scan a defined area with constant angle (circular transects) to the base station (point of interest).
+
+    \sa WimaArea
+*/
+
+

src/Wima/CircularSurvey.cc

@@ -387,11 +387,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
         snake::BoostPolygon polygonENU;
         snake::BoostPoint originENU{0, 0};
         snake::areaToEnu(origin, polygon, polygonENU);
+        std::string error;
         // Check validity.
-        if (!bg::is_valid(polygonENU)) {
+        if (!bg::is_valid(polygonENU, error)) {
 #ifdef DEBUG_CIRCULAR_SURVEY
           qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
                         "invalid polygon.";
+          qWarning() << error.c_str();
 #endif
           return PtrTransects();
         } else {
@@ -401,21 +403,27 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
           std::vector<snake::Angle> angles;
           angles.reserve(polygonENU.outer().size());
           for (const auto &p : polygonENU.outer()) {
-            distances.push_back(bg::distance(originENU, p) * si::meter);
-            angles.push_back((std::atan2(p.get<1>(), p.get<0>()) + M_PI) *
-                             si::radian);
+            snake::Length distance = bg::distance(originENU, p) * si::meter;
+            distances.push_back(distance);
+            snake::Angle alpha =
+                (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
+            alpha =
+                alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
+            angles.push_back(alpha);
+#ifdef DEBUG_CIRCULAR_SURVEY
+            qWarning() << "distances, angles, coordinates:";
+            qWarning() << to_string(distance).c_str();
+            qWarning() << to_string(snake::Degree(alpha)).c_str();
+            qWarning() << "x = " << p.get<0>() << "y = " << p.get<1>();
+#endif
           }
 
           auto rMin = deltaR; // minimal circle radius
           snake::Angle alpha1(0 * degree::degree);
           snake::Angle alpha2(360 * degree::degree);
-          bool refInside = true;
           // Determine r_min by successive approximation
           if (!bg::within(originENU, polygonENU)) {
             rMin = bg::distance(originENU, polygonENU) * si::meter;
-            alpha1 = (*std::min_element(angles.begin(), angles.end()));
-            alpha2 = (*std::max_element(angles.begin(), angles.end()));
-            refInside = false;
           }
 
           auto rMax =
@@ -431,44 +439,31 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
               ClipperLib::cInt(std::round(originENU.get<0>())),
               ClipperLib::cInt(std::round(originENU.get<1>()))};
 
+#ifdef SHOW_CIRCULAR_SURVEY_TIME
+          auto s1 = std::chrono::high_resolution_clock::now();
+#endif
           // Generate circle sectors.
           auto rScaled = rMinScaled;
-          const auto nTran = long(std::floor(((rMax - rMin) / deltaR).value()));
+          const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
           vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
           const auto nSectors =
               long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
 #ifdef DEBUG_CIRCULAR_SURVEY
           qWarning() << "sector parameres:";
-          qWarning() << "alpha1: " << to_string(alpha1).c_str();
-          qWarning() << "alpha2: " << to_string(alpha2).c_str();
+          qWarning() << "alpha1: " << to_string(snake::Degree(alpha1)).c_str();
+          qWarning() << "alpha2: " << to_string(snake::Degree(alpha2)).c_str();
           qWarning() << "n: "
                      << to_string((alpha2 - alpha1) / deltaAlpha).c_str();
           qWarning() << "nSectors: " << nSectors;
           qWarning() << "rMin: " << to_string(rMin).c_str();
           qWarning() << "rMax: " << to_string(rMax).c_str();
           qWarning() << "nTran: " << nTran;
-          qWarning() << "refInside: " << refInside;
 #endif
           using ClipperCircle =
               GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
-          // Helper lambda.
-          std::function<void(ClipperCircle & circle,
-                             ClipperLib::Path & sectors)>
-              approx;
-          if (refInside) {
-            approx = [nSectors](ClipperCircle &circle,
-                                ClipperLib::Path &sector) {
-              approximate(circle, nSectors, sector);
-            };
-          } else {
-            approx = [nSectors, alpha1, alpha2](ClipperCircle &circle,
-                                                ClipperLib::Path &sector) {
-              approximate(circle, nSectors, alpha1, alpha2, sector);
-            };
-          }
           for (auto &sector : sectors) {
             ClipperCircle circle(rScaled, originScaled);
-            approx(circle, sector);
+            approximate(circle, nSectors, sector);
             rScaled += deltaRScaled;
           }
           // Clip sectors to polygonENU.
@@ -502,6 +497,21 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
               transectsENU.push_back(transect);
             }
           }
+          // Join sectors which where slit due to clipping.
+          static_assert(false, "continue here.");
+          for (std::size_t i = 0; i < transectsENU.size() - 1; ++i) {
+            const auto &t1 = transectsENU[i];
+            for (std::size_t j = i + 1; j < transectsENU.size(); ++j) {
+              const auto &t2 = transectsENU[j];
+            }
+          }
+#ifdef SHOW_CIRCULAR_SURVEY_TIME
+          qWarning() << "CircularSurvey: concurrent update transect gen. time: "
+                     << std::chrono::duration_cast<std::chrono::milliseconds>(
+                            std::chrono::high_resolution_clock::now() - s1)
+                            .count()
+                     << " ms";
+#endif
 
           if (transectsENU.size() == 0) {
 #ifdef DEBUG_CIRCULAR_SURVEY
@@ -512,10 +522,10 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
           }
 
           // Route transects;
-          snake::Transects transectsRouted;
+          std::vector<snake::TransectInfo> transectsInfo;
           snake::Route route;
           std::string errorString;
-          bool success = snake::route(polygonENU, transectsENU, transectsRouted,
+          bool success = snake::route(polygonENU, transectsENU, transectsInfo,
                                       route, errorString);
           if (!success) {
 #ifdef DEBUG_CIRCULAR_SURVEY
@@ -525,6 +535,18 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
             return PtrTransects();
           }
 
+          // Remove return path.
+          const auto &info = transectsInfo.back();
+          const auto &lastTransect = transectsENU[info.index];
+          const auto &lastWaypoint =
+              info.reversed ? lastTransect.front() : lastTransect.back();
+          auto &wp = route.back();
+          while (wp != lastWaypoint) {
+            route.pop_back();
+            wp = route.back();
+          }
+
+          // Convert to geo coordinates.
           QList<CoordInfo_t> transectList;
           transectList.reserve(route.size());
           for (const auto &vertex : route) {
@@ -533,13 +555,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
             CoordInfo_t coordinfo = {c, CoordTypeInterior};
             transectList.append(coordinfo);
           }
-          PtrTransects transects(new Transects());
-          transects->append(transectList);
+          PtrTransects pTransects(new Transects());
+          pTransects->append(transectList);
 #ifdef DEBUG_CIRCULAR_SURVEY
           qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
                         "concurrent update success.";
 #endif
-          return transects;
+          return pTransects;
         }
       }); // QtConcurrent::run()
       _watcher.setFuture(future);

src/Wima/Snake/SnakeThread.cc

@@ -360,7 +360,7 @@ void SnakeThread::run() {
 
   bool waypointsValid = true;
   snake::Transects transects;
-  snake::Transects transectsRouted;
+  std::vector<snake::TransectInfo> transectsInfo;
   snake::Route route;
   bool needWaypointUpdate = this->pImpl->input.scenarioChanged ||
                             this->pImpl->input.routeParametersChanged ||
@@ -385,8 +385,8 @@ void SnakeThread::run() {
         waypointsValid = false;
       } else if (transects.size() > 1) {
         // Route transects.
-        success = snake::route(this->pImpl->jAreaENU, transects,
-                               transectsRouted, route, errorString);
+        success = snake::route(this->pImpl->jAreaENU, transects, transectsInfo,
+                               route, errorString);
         if (!success) {
           UniqueLock lk(this->pImpl->output.mutex);
           this->pImpl->output.errorMessage = errorString.c_str();
@@ -481,7 +481,10 @@ void SnakeThread::run() {
     this->pImpl->output.waypoints.clear();
     this->pImpl->output.waypointsENU.clear();
     // Store arrival path.
-    const auto &firstWaypoint = transectsRouted.front().front();
+    const auto &info = transectsInfo.front();
+    const auto &firstTransect = transects[info.index];
+    const auto &firstWaypoint =
+        info.reversed ? firstTransect.front() : firstTransect.back();
     long startIdx = 0;
     for (long i = 0; i < long(route.size()); ++i) {
       const auto &boostVertex = route[i];
@@ -497,7 +500,10 @@ void SnakeThread::run() {
     }
     // Store return path.
     long endIdx = 0;
-    const auto &lastWaypoint = transectsRouted.back().back();
+    const auto &info2 = transectsInfo.back();
+    const auto &lastTransect = transects[info2.index];
+    const auto &lastWaypoint =
+        info2.reversed ? lastTransect.front() : lastTransect.back();
     for (long i = route.size() - 1; i >= 0; --i) {
       const auto &boostVertex = route[i];
       if (boostVertex == lastWaypoint) {

src/Wima/Snake/route.cpp

-bool flight_plan::route(const BoostPolygon &area,
-                        const flight_plan::Transects &transects,
-                        Transects &transectsRouted, flight_plan::Route &route,
-                        string &errorString) {
-  //=======================================
-  // Route Transects using Google or-tools.
-  //=======================================
-
-  // Create vertex list;
-  BoostLineString vertices;
-  size_t n0 = 0;
-  for (const auto &t : transects) {
-    n0 += std::min<std::size_t>(t.size(), 2);
-  }
-  vertices.reserve(n0);
-
-  struct TransectInfo {
-    TransectInfo(size_t n, bool f) : index(n), front(f) {}
-    size_t index;
-    bool front;
-  };
-  std::vector<TransectInfo> transectInfoList;
-  for (size_t i = 0; i < transects.size(); ++i) {
-    const auto &t = transects[i];
-    vertices.push_back(t.front());
-    transectInfoList.push_back(TransectInfo{i, true});
-    if (t.size() >= 2) {
-      vertices.push_back(t.back());
-      transectInfoList.push_back(TransectInfo{i, false});
-    }
-  }
-
-  for (long i = 0; i < long(area.outer().size()) - 1; ++i) {
-    vertices.push_back(area.outer()[i]);
-  }
-  for (auto &ring : area.inners()) {
-    for (auto &vertex : ring)
-      vertices.push_back(vertex);
-  }
-  size_t n1 = vertices.size();
-  // Generate routing model.
-  RoutingDataModel dataModel;
-  Matrix<double> connectionGraph(n1, n1);
-  // Offset joined area.
-  BoostPolygon areaOffset;
-  offsetPolygon(area, areaOffset, detail::offsetConstant);
-#ifdef SNAKE_SHOW_TIME
-  auto start = std::chrono::high_resolution_clock::now();
-#endif
-  generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph);
-#ifdef SNAKE_SHOW_TIME
-  auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
-      std::chrono::high_resolution_clock::now() - start);
-  cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms"
-       << endl;
-#endif
-
-  // Create Routing Index Manager.
-  RoutingIndexManager manager(dataModel.distanceMatrix.getN(),
-                              dataModel.numVehicles, dataModel.depot);
-  // Create Routing Model.
-  RoutingModel routing(manager);
-
-  // Create and register a transit callback.
-  const int transitCallbackIndex = routing.RegisterTransitCallback(
-      [&dataModel, &manager](int64 from_index, int64 to_index) -> int64 {
-        // Convert from routing variable Index to distance matrix NodeIndex.
-        auto from_node = manager.IndexToNode(from_index).value();
-        auto to_node = manager.IndexToNode(to_index).value();
-        return dataModel.distanceMatrix.get(from_node, to_node);
-      });
-
-  // Define cost of each arc.
-  routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);
-
-  // Define Constraints (this constraints have a huge impact on the
-  // solving time, improvments could be done, e.g. SearchFilter).
-#ifdef SNAKE_DEBUG
-  std::cout << "snake::route(): Constraints:" << std::endl;
-#endif
-  Solver *solver = routing.solver();
-  size_t index = 0;
-  for (size_t i = 0; i < transects.size(); ++i) {
-    const auto &t = transects[i];
-#ifdef SNAKE_DEBUG
-    std::cout << "i = " << i << ": t.size() = " << t.size() << std::endl;
-#endif
-    if (t.size() >= 2) {
-      auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index));
-      auto idx1 =
-          manager.NodeToIndex(RoutingIndexManager::NodeIndex(index + 1));
-      auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
-      auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
-      auto c = solver->MakeNonEquality(cond0, cond1);
-      solver->AddConstraint(c);
-#ifdef SNAKE_DEBUG
-      std::cout << "( next(" << index << ") == " << index + 1 << " ) != ( next("
-                << index + 1 << ") == " << index << " )" << std::endl;
-#endif
-      index += 2;
-    } else {
-      index += 1;
-    }
-  }
-
-  // Setting first solution heuristic.
-  RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
-  searchParameters.set_first_solution_strategy(
-      FirstSolutionStrategy::PATH_CHEAPEST_ARC);
-  google::protobuf::Duration *tMax =
-      new google::protobuf::Duration(); // seconds
-  tMax->set_seconds(10);
-  searchParameters.set_allocated_time_limit(tMax);
-
-  // Solve the problem.
-#ifdef SNAKE_SHOW_TIME
-  start = std::chrono::high_resolution_clock::now();
-#endif
-  const Assignment *solution = routing.SolveWithParameters(searchParameters);
-#ifdef SNAKE_SHOW_TIME
-  delta = std::chrono::duration_cast<std::chrono::milliseconds>(
-      std::chrono::high_resolution_clock::now() - start);
-  cout << "Execution time routing.SolveWithParameters(): " << delta.count()
-       << " ms" << endl;
-#endif
-
-  if (!solution || solution->Size() <= 1) {
-    errorString = "Not able to solve the routing problem.";
-    return false;
-  }
-
-  // Extract index list from solution.
-  index = routing.Start(0);
-  std::vector<size_t> route_idx;
-  route_idx.push_back(manager.IndexToNode(index).value());
-  while (!routing.IsEnd(index)) {
-    index = solution->Value(routing.NextVar(index));
-    route_idx.push_back(manager.IndexToNode(index).value());
-  }
-
-  // Helper Lambda.
-  auto idx2Vertex = [&vertices](const std::vector<size_t> &idxArray,
-                                std::vector<BoostPoint> &path) {
-    for (auto idx : idxArray)
-      path.push_back(vertices[idx]);
-  };
-
-  // Construct route.
-  for (size_t i = 0; i < route_idx.size() - 1; ++i) {
-    size_t idx0 = route_idx[i];
-    size_t idx1 = route_idx[i + 1];
-    const auto &info1 = transectInfoList[idx0];
-    const auto &info2 = transectInfoList[idx1];
-    if (info1.index == info2.index) { // same transect?
-      if (!info1.front) {             // transect reversal needed?
-        BoostLineString reversedTransect;
-        const auto &t = transects[info1.index];
-        for (auto it = t.end() - 1; it >= t.begin(); --it) {
-          reversedTransect.push_back(*it);
-        }
-        transectsRouted.push_back(reversedTransect);
-        for (auto it = reversedTransect.begin();
-             it < reversedTransect.end() - 1; ++it) {
-          route.push_back(*it);
-        }
-      } else {
-        const auto &t = transects[info1.index];
-        for (auto it = t.begin(); it < t.end() - 1; ++it) {
-          route.push_back(*it);
-        }
-        transectsRouted.push_back(t);
-      }
-    } else {
-      std::vector<size_t> idxList;
-      shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
-      if (i != route_idx.size() - 2) {
-        idxList.pop_back();
-      }
-      idx2Vertex(idxList, route);
-    }
-  }
-  return true;
-}

src/Wima/Snake/snake.cpp

@@ -781,7 +781,8 @@ void generateRoutingModel(const BoostLineString &vertices,
 }
 
 bool route(const BoostPolygon &area, const Transects &transects,
-           Transects &transectsRouted, Route &route, string &errorString) {
+           std::vector<TransectInfo> &transectInfo, Route &route,
+           string &errorString) {
   //=======================================
   // Route Transects using Google or-tools.
   //=======================================
@@ -794,19 +795,19 @@ bool route(const BoostPolygon &area, const Transects &transects,
   }
   vertices.reserve(n0);
 
-  struct TransectInfo {
-    TransectInfo(size_t n, bool f) : index(n), front(f) {}
+  struct LocalInfo {
+    LocalInfo(size_t n, bool f) : index(n), front(f) {}
     size_t index;
     bool front;
   };
-  std::vector<TransectInfo> transectInfoList;
+  std::vector<LocalInfo> localTransectInfo;
   for (size_t i = 0; i < transects.size(); ++i) {
     const auto &t = transects[i];
     vertices.push_back(t.front());
-    transectInfoList.push_back(TransectInfo{i, true});
+    localTransectInfo.push_back(LocalInfo{i, true});
     if (t.size() >= 2) {
       vertices.push_back(t.back());
-      transectInfoList.push_back(TransectInfo{i, false});
+      localTransectInfo.push_back(LocalInfo{i, false});
     }
   }
 
@@ -929,16 +930,18 @@ bool route(const BoostPolygon &area, const Transects &transects,
   for (size_t i = 0; i < route_idx.size() - 1; ++i) {
     size_t idx0 = route_idx[i];
     size_t idx1 = route_idx[i + 1];
-    const auto &info1 = transectInfoList[idx0];
-    const auto &info2 = transectInfoList[idx1];
+    const auto &info1 = localTransectInfo[idx0];
+    const auto &info2 = localTransectInfo[idx1];
     if (info1.index == info2.index) { // same transect?
-      if (!info1.front) {             // transect reversal needed?
+      TransectInfo trInfo(info1.index,
+                          !info1.front ? true : false /*transect reversed?*/);
+      transectInfo.push_back(trInfo);
+      if (!info1.front) { // transect reversal needed?
         BoostLineString reversedTransect;
         const auto &t = transects[info1.index];
         for (auto it = t.end() - 1; it >= t.begin(); --it) {
           reversedTransect.push_back(*it);
         }
-        transectsRouted.push_back(reversedTransect);
         for (auto it = reversedTransect.begin();
              it < reversedTransect.end() - 1; ++it) {
           route.push_back(*it);
@@ -948,7 +951,6 @@ bool route(const BoostPolygon &area, const Transects &transects,
         for (auto it = t.begin(); it < t.end() - 1; ++it) {
           route.push_back(*it);
         }
-        transectsRouted.push_back(t);
       }
     } else {
       std::vector<size_t> idxList;

src/Wima/Snake/snake.h

@@ -98,7 +98,9 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
                                      origin.altitude(), earth);
 
   double x = 0, y = 0, z = 0;
-  proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z);
+  auto alt = in.altitude();
+  alt = std::isnan(alt) ? 0 : alt;
+  proj.Forward(in.latitude(), in.longitude(), alt, x, y, z);
   out.set<0>(x);
   out.set<1>(y);
   (void)z;
@@ -156,6 +158,8 @@ void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
 typedef bu::quantity<bu::si::length> Length;
 typedef bu::quantity<bu::si::area> Area;
 typedef bu::quantity<bu::si::plane_angle> Angle;
+typedef bu::quantity<bu::si::plane_angle> Radian;
+typedef bu::quantity<bu::degree::plane_angle> Degree;
 
 bool joinedArea(const std::vector<BoostPolygon *> &areas, BoostPolygon &jArea);
 bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea,
@@ -174,12 +178,19 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle,
                            const std::vector<BoostPolygon> &tiles,
                            const Progress &p, Transects &t,
                            string &errorString);
+
+struct TransectInfo {
+  TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
+  size_t index;
+  bool reversed;
+};
 bool route(const BoostPolygon &area, const Transects &transects,
-           Transects &transectsRouted, Route &route, string &errorString);
+           std::vector<TransectInfo> &transectInfo, Route &route,
+           string &errorString);
 
 namespace detail {
 const double offsetConstant =
-    1; // meter, polygon offset to compenstate for numerical inaccurracies.
+    0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
 } // namespace detail
 } // namespace snake
 

src/Wima/Snake/snake_old.cpp

-bool FlightPlan::generate(double lineDistance, double minTransectLength)
-{
-    _waypointsENU.clear();    
-    _waypoints.clear();
-    _arrivalPathIdx.clear();
-    _returnPathIdx.clear();
-
-#ifndef NDEBUG
-    _PathVertices.clear();
-#endif
-#ifdef SHOW_TIME
-    auto start = std::chrono::high_resolution_clock::now();
-#endif
-    if (!_generateTransects(lineDistance, minTransectLength))
-        return false;
-#ifdef SHOW_TIME
-    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
-    cout << endl;
-   cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl;
-#endif
-
-    //=======================================
-    // Route Transects using Google or-tools.
-    //=======================================
-    // Offset joined area.
-    const BoostPolygon &jArea = _scenario.getJoineAreaENU();
-    BoostPolygon jAreaOffset;
-    offsetPolygon(jArea, jAreaOffset, detail::offsetConstant);
-
-    // Create vertex list;
-    BoostLineString vertices;
-    size_t n_t = _transects.size()*2;
-    size_t n0  = n_t+1;
-    vertices.reserve(n0);
-    for (auto lstring : _transects){
-        for (auto vertex : lstring){
-            vertices.push_back(vertex);
-        }
-    }
-    vertices.push_back(_scenario.getHomePositonENU());
-
-    for (long i=0; i<long(jArea.outer().size())-1; ++i) {
-        vertices.push_back(jArea.outer()[i]);
-    }
-    for (auto ring : jArea.inners()) {
-        for (auto vertex : ring)
-            vertices.push_back(vertex);
-    }
-    size_t n1 = vertices.size();
-    // Generate routing model.
-    RoutingDataModel_t dataModel;
-    Matrix<double> connectionGraph(n1, n1);
-
-#ifdef SHOW_TIME
-    start = std::chrono::high_resolution_clock::now();
-#endif
-    _generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
-#ifdef SHOW_TIME
-    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
-    cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl;
-#endif
-
-    // Create Routing Index Manager.
-    RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles,
-                                dataModel.depot);
-    // Create Routing Model.
-    RoutingModel routing(manager);
-
-    // Create and register a transit callback.
-    const int transit_callback_index = routing.RegisterTransitCallback(
-        [&dataModel, &manager](int64 from_index, int64 to_index) -> int64 {
-          // Convert from routing variable Index to distance matrix NodeIndex.
-          auto from_node = manager.IndexToNode(from_index).value();
-          auto to_node = manager.IndexToNode(to_index).value();
-          return dataModel.distanceMatrix.get(from_node, to_node);
-        });
-
-    // Define cost of each arc.
-    routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);
-
-
-    // Define Constraints.
-    size_t n = _transects.size()*2;
-    Solver *solver = routing.solver();
-    for (size_t i=0; i<n; i=i+2){
-//        auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
-//        auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
-//        auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
-//        auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
-//        auto c = solver->MakeNonEquality(cond0, cond1);
-//        solver->AddConstraint(c);
-
-        // alternative
-        auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
-        auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
-        auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
-        auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
-        vector<IntVar*> conds{cond0, cond1};
-        auto c = solver->MakeAllDifferent(conds);
-        solver->MakeRejectFilter();
-        solver->AddConstraint(c);
-    }
-
-
-    // Setting first solution heuristic.
-    RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
-    searchParameters.set_first_solution_strategy(
-        FirstSolutionStrategy::PATH_CHEAPEST_ARC);
-    google::protobuf::Duration *tMax = new google::protobuf::Duration(); // seconds
-    tMax->set_seconds(10);
-    searchParameters.set_allocated_time_limit(tMax);
-
-    // Solve the problem.
-#ifdef SHOW_TIME
-    start = std::chrono::high_resolution_clock::now();
-#endif
-    const Assignment* solution = routing.SolveWithParameters(searchParameters);
-#ifdef SHOW_TIME
-    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
-    cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl;
-#endif
-
-    if (!solution || solution->Size() <= 1){
-        errorString = "Not able to solve the routing problem.";
-        return false;
-    }
-
-    // Extract waypoints from solution.
-    long index = routing.Start(0);
-    std::vector<size_t> route;
-    route.push_back(manager.IndexToNode(index).value());
-    while (!routing.IsEnd(index)){
-        index = solution->Value(routing.NextVar(index));
-        route.push_back(manager.IndexToNode(index).value());
-    }
-
-    // Connect transects
-#ifndef NDEBUG
-    _PathVertices = vertices;
-#endif
-
-    {
-    _waypointsENU.push_back(vertices[route[0]]);
-    vector<size_t> pathIdx;
-    long arrivalPathLength = 0;
-    for (long i=0; i<long(route.size())-1; ++i){
-        size_t idx0 = route[i];
-        size_t idx1 = route[i+1];
-        pathIdx.clear();
-        shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
-        if ( i==0 )
-            arrivalPathLength = pathIdx.size();
-        for (size_t j=1; j<pathIdx.size(); ++j)
-            _waypointsENU.push_back(vertices[pathIdx[j]]);
-    }
-
-
-    long returnPathLength = pathIdx.size();
-    for (long i=returnPathLength; i > 0; --i)
-        _returnPathIdx.push_back(_waypointsENU.size()-i);
-
-    for (long i=0; i < arrivalPathLength; ++i)
-        _arrivalPathIdx.push_back(i);
-    }
-
-    // Back transform waypoints.
-    GeoPoint3D origin{_scenario.getOrigin()};
-    for (auto vertex : _waypointsENU) {
-        GeoPoint3D geoVertex;
-        fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
-        _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
-    }
-
-    return true;
-}
-
-bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength)
-{
-    _transects.clear();
-    if (_scenario.getTilesENU().size() != _progress.size()){
-        ostringstream strstream;
-        strstream << "Number of tiles ("
-                  << _scenario.getTilesENU().size()
-                  << ") is not equal to progress array length ("
-                  << _progress.size()
-                  << ")";
-        errorString = strstream.str();
-        return false;
-    }
-
-    // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
-    size_t num_tiles = _progress.size();
-    vector<BoostPolygon> processedTiles;
-    {
-        const auto &tiles = _scenario.getTilesENU();
-        for (size_t i=0; i<num_tiles; ++i) {
-            if (_progress[i] == 100){
-                processedTiles.push_back(tiles[i]);
-            }
-        }
-
-        if (processedTiles.size() == num_tiles)
-            return true;
-    }
-
-    // Convert measurement area and tiles to clipper path.
-    ClipperLib::Path mAreaClipper;
-    for ( auto vertex : _scenario.getMeasurementAreaENU().outer() ){
-        mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
-                                                    static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
-    }
-    vector<ClipperLib::Path> processedTilesClipper;
-    for (auto t : processedTiles){
-        ClipperLib::Path path;
-        for (auto vertex : t.outer()){
-            path.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
-                                                static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
-        }
-        processedTilesClipper.push_back(path);
-    }
-
-    const min_bbox_rt &bbox     = _scenario.getMeasurementAreaBBoxENU();
-    double alpha                = bbox.angle;
-    double x0                   = bbox.corners.outer()[0].get<0>();
-    double y0                   = bbox.corners.outer()[0].get<1>();
-    double bboxWidth            = bbox.width;
-    double bboxHeight           = bbox.height;
-    double delta                = detail::offsetConstant;
-
-    size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects
-    vector<double> yCoords;
-    yCoords.reserve(num_t);
-    double y = -delta;
-    for (size_t i=0; i < num_t; ++i) {
-        yCoords.push_back(y);
-        y += lineDistance;
-    }
-
-
-    // Generate transects and convert them to clipper path.
-    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-alpha*180/M_PI);
-    trans::translate_transformer<double, 2, 2> translate_back(x0, y0);
-    vector<ClipperLib::Path> transectsClipper;
-    transectsClipper.reserve(num_t);
-    for (size_t i=0; i < num_t; ++i) {
-        // calculate transect
-        BoostPoint v1{-delta, yCoords[i]};
-        BoostPoint v2{bboxWidth+delta, yCoords[i]};
-        BoostLineString transect;
-        transect.push_back(v1);
-        transect.push_back(v2);
-
-        // transform back
-        BoostLineString temp_transect;
-        bg::transform(transect, temp_transect, rotate_back);
-        transect.clear();
-        bg::transform(temp_transect, transect, translate_back);
-
-
-        ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(transect[0].get<0>()*CLIPPER_SCALE),
-                                static_cast<ClipperLib::cInt>(transect[0].get<1>()*CLIPPER_SCALE)};
-        ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(transect[1].get<0>()*CLIPPER_SCALE),
-                                static_cast<ClipperLib::cInt>(transect[1].get<1>()*CLIPPER_SCALE)};
-        ClipperLib::Path path{c1, c2};
-        transectsClipper.push_back(path);
-    }
-
-    // Perform clipping.
-    // Clip transects to measurement area.
-    ClipperLib::Clipper clipper;
-    clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
-    clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
-    ClipperLib::PolyTree clippedTransecsPolyTree1;
-    clipper.Execute(ClipperLib::ctIntersection, clippedTransecsPolyTree1, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
-
-    // Subtract holes (tiles with measurement_progress == 100) from transects.
-    clipper.Clear();
-    for (auto child : clippedTransecsPolyTree1.Childs)
-        clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
-    clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
-    ClipperLib::PolyTree clippedTransecsPolyTree2;
-    clipper.Execute(ClipperLib::ctDifference, clippedTransecsPolyTree2, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
-
-    // Extract transects from  PolyTree and convert them to BoostLineString
-    for (auto child : clippedTransecsPolyTree2.Childs){
-        ClipperLib::Path clipperTransect = child->Contour;
-        BoostPoint v1{static_cast<double>(clipperTransect[0].X)/CLIPPER_SCALE,
-                      static_cast<double>(clipperTransect[0].Y)/CLIPPER_SCALE};
-        BoostPoint v2{static_cast<double>(clipperTransect[1].X)/CLIPPER_SCALE,
-                      static_cast<double>(clipperTransect[1].Y)/CLIPPER_SCALE};
-
-        BoostLineString transect{v1, v2};
-        if (bg::length(transect) >= minTransectLength)
-            _transects.push_back(transect);
-
-    }
-
-    if (_transects.size() < 1)
-        return false;
-
-    return true;
-}
-
-void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
-                                       const BoostPolygon &polygonOffset,
-                                       size_t n0,
-                                       FlightPlan::RoutingDataModel_t &dataModel,
-                                       Matrix<double> &graph)
-{
-#ifdef SHOW_TIME
-    auto start = std::chrono::high_resolution_clock::now();
-#endif
-    graphFromPolygon(polygonOffset, vertices, graph);
-#ifdef SHOW_TIME
-    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
-    cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
-#endif
-//    cout << endl;
-//    for (size_t i=0; i<graph.size(); ++i){
-//        vector<double> &row = graph[i];
-//        for (size_t j=0; j<row.size();++j){
-//            cout << "(" << i << "," << j << "):" << row[j] << " ";
-//        }
-//        cout << endl;
-//    }
-//    cout << endl;
-    Matrix<double> distanceMatrix(graph);
-#ifdef SHOW_TIME
-    start = std::chrono::high_resolution_clock::now();
-#endif
-    toDistanceMatrix(distanceMatrix);
-#ifdef SHOW_TIME
-    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
-    cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;
-#endif
-
-    dataModel.distanceMatrix.setDimension(n0, n0);
-    for (size_t i=0; i<n0; ++i){
-        dataModel.distanceMatrix.set(i, i, 0);
-        for (size_t j=i+1; j<n0; ++j){
-            dataModel.distanceMatrix.set(i, j, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
-            dataModel.distanceMatrix.set(j, i, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
-        }
-    }
-
-    dataModel.numVehicles   = 1;
-    dataModel.depot         = n0-1;
-}