diff --git a/src/Wima/CS_old.cc b/src/Wima/CS_old.cc new file mode 100644 index 0000000000000000000000000000000000000000..35a3be11acebd4cda5c02238c488319227bc594b --- /dev/null +++ b/src/Wima/CS_old.cc @@ -0,0 +1,737 @@ +#include "CircularSurveyComplexItem.h" +#include "JsonHelper.h" +#include "QGCApplication.h" +#include + + +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 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 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 &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& 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& 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& 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::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> transectPath; + if(!_generateTransectPath(transectPath, surveyPolygon)) + return; + + /// optimize path to snake or zig-zag pattern + bool fixedDirectionBool = _fixedDirection.rawValue().toBool(); + QVector currentSection = transectPath.takeFirst(); + if ( currentSection.isEmpty() ) + return; + + QVector optiPath; // optimized path + while( !transectPath.empty() ) { + optiPath.append(currentSection); + QPointF endVertex = currentSection.last(); + double minDist = std::numeric_limits::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> transectsReverse; + transectsReverse.reserve(_transects.size()); + for (auto list : _transects) { + QList 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> transectPath; + if(!_generateTransectPath(transectPath, surveyPolygon)) + return; + + // optimize path to snake or zig-zag pattern + const bool fixedDirectionBool = _fixedDirection.rawValue().toBool(); + QVector currentSection = transectPath.takeFirst(); if ( currentSection.isEmpty() ) return; + QVector 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::infinity(); + int index = 0; + + // iterate over all paths in fullPath and assign the one with the shortest distance to endVertex to currentSection + QVector connectorPath; + for (int i = 0; i < transectPath.size(); i++) { + QVector iteratorPath = transectPath[i]; + QVector 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().distanceTo(_visualTransectPoints[i+1].value()); + } + 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 &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 path2D; + + bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); + if (retVal) + toGeoList(path2D, /*origin*/ start, shortestPath); + + return retVal; +} + +bool CircularSurveyComplexItem::_generateTransectPath(QVector > &transectPath, const QPolygonF &surveyPolygon) +{ + using namespace PlanimetryCalculus; + QVector 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 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 intersectPoints; + QVector typeList; + QVector> neighbourList; + if (intersects(circle, surveyPolygon, intersectPoints, neighbourList, typeList)) { + + // intersection Points between circle and polygon, entering polygon + // when walking in counterclockwise direction along circle + QVector entryPoints; + // intersection Points between circle and polygon, leaving polygon + // when walking in counterclockwise direction along circle + QVector exitPoints; + // determine entryPoints and exit Points + for (int j = 0; j < intersectPoints.size(); j++) { + QVector 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::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 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 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 &path, const QGeoCoordinate &reference) +{ + using namespace GeoUtilities; + + QVector geoPath; + toGeoList(path, reference, geoPath); + QList 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 +*/ + + diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index 35710bcf4d138957cd599c13b8addfa07ec4a7d6..4a406ec2a1686a023e95bdf9b6281ecf7d437dc8 100644 --- a/src/Wima/CircularSurvey.cc +++ b/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 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 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; - // Helper lambda. - std::function - approx; - if (refInside) { - approx = [nSectors](ClipperCircle &circle, - ClipperLib::Path §or) { - approximate(circle, nSectors, sector); - }; - } else { - approx = [nSectors, alpha1, alpha2](ClipperCircle &circle, - ClipperLib::Path §or) { - approximate(circle, nSectors, alpha1, alpha2, sector); - }; - } for (auto §or : 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::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 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 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); diff --git a/src/Wima/Snake/SnakeThread.cc b/src/Wima/Snake/SnakeThread.cc index 3d683e5f753f2adcdbaa865cf369c36e905975c9..1242e4e82cbcd3ad854b9d0cbb4688844384b1de 100644 --- a/src/Wima/Snake/SnakeThread.cc +++ b/src/Wima/Snake/SnakeThread.cc @@ -360,7 +360,7 @@ void SnakeThread::run() { bool waypointsValid = true; snake::Transects transects; - snake::Transects transectsRouted; + std::vector 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) { diff --git a/src/Wima/Snake/route.cpp b/src/Wima/Snake/route.cpp deleted file mode 100644 index 59b62814e5c908d698d91b1250310608f5e9c4cd..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/route.cpp +++ /dev/null @@ -1,183 +0,0 @@ -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(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 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 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::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::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 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 &idxArray, - std::vector &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 idxList; - shortestPathFromGraph(connectionGraph, idx0, idx1, idxList); - if (i != route_idx.size() - 2) { - idxList.pop_back(); - } - idx2Vertex(idxList, route); - } - } - return true; -} diff --git a/src/Wima/Snake/snake.cpp b/src/Wima/Snake/snake.cpp index a2093df46599ece9e654a5ae1c5338d57cefc2ed..eca90645565e83492dbd8b0fb7f5617e84cb5877 100644 --- a/src/Wima/Snake/snake.cpp +++ b/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, 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 transectInfoList; + std::vector 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 idxList; diff --git a/src/Wima/Snake/snake.h b/src/Wima/Snake/snake.h index 806923c2c5e8f6d096ae4b6267f4387b64ea3d2d..7b710c9b39fb086764e52783ffd8b3e7e0912171 100644 --- a/src/Wima/Snake/snake.h +++ b/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 &graph, size_t startIndex, typedef bu::quantity Length; typedef bu::quantity Area; typedef bu::quantity Angle; +typedef bu::quantity Radian; +typedef bu::quantity Degree; bool joinedArea(const std::vector &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 &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, 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 diff --git a/src/Wima/Snake/snake_old.cpp b/src/Wima/Snake/snake_old.cpp deleted file mode 100644 index ffa2a152a96115e788a269b6a64b0f10960bf55b..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/snake_old.cpp +++ /dev/null @@ -1,348 +0,0 @@ -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::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 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::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; iIsEqual(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 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::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 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 pathIdx; - long arrivalPathLength = 0; - for (long i=0; 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 processedTiles; - { - const auto &tiles = _scenario.getTilesENU(); - for (size_t i=0; i(vertex.get<0>()*CLIPPER_SCALE), - static_cast(vertex.get<1>()*CLIPPER_SCALE)}); - } - vector processedTilesClipper; - for (auto t : processedTiles){ - ClipperLib::Path path; - for (auto vertex : t.outer()){ - path.push_back(ClipperLib::IntPoint{static_cast(vertex.get<0>()*CLIPPER_SCALE), - static_cast(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 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 rotate_back(-alpha*180/M_PI); - trans::translate_transformer translate_back(x0, y0); - vector 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(transect[0].get<0>()*CLIPPER_SCALE), - static_cast(transect[0].get<1>()*CLIPPER_SCALE)}; - ClipperLib::IntPoint c2{static_cast(transect[1].get<0>()*CLIPPER_SCALE), - static_cast(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(clipperTransect[0].X)/CLIPPER_SCALE, - static_cast(clipperTransect[0].Y)/CLIPPER_SCALE}; - BoostPoint v2{static_cast(clipperTransect[1].X)/CLIPPER_SCALE, - static_cast(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 &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::high_resolution_clock::now()-start); - cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl; -#endif -// cout << endl; -// for (size_t i=0; i &row = graph[i]; -// for (size_t j=0; j 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::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