Skip to content

Q
qgroundcontrol
Commit 7271b853 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

circular survey optimized, some bugs though

parent a22db7f4
Show whitespace changes
Inline Side-by-side
Showing with 2959 additions and 8381 deletions
Paths/KlingenbachTest.wima
View file @ 7271b853
This source diff could not be displayed because it is too large. You can view the blob instead.
qgroundcontrol.pro
View file @ 7271b853
......@@ -29,7 +29,9 @@ DebugBuild {
DESTDIR = $${OUT_PWD}/debug
DEFINES += DEBUG
DEFINES += SNAKE_SHOW_TIME
DEFINES += SNAKE_DEBUG
#DEFINES += SNAKE_DEBUG
DEFINES += SHOW_CIRCULAR_SURVEY_TIME
DEFINES += DEBUG_CIRCULAR_SURVEY
#DEFINES += ROS_BRIDGE_DEBUG
}
else {
......@@ -411,6 +413,8 @@ FORMS += \
#
HEADERS += \
src/Wima/CircularSurvey.h \
src/Wima/Geometry/GenericCircle.h \
src/Wima/Snake/clipper/clipper.hpp \
src/Wima/Snake/mapbox/feature.hpp \
src/Wima/Snake/mapbox/geometry.hpp \
......@@ -473,9 +477,7 @@ HEADERS += \
src/Wima/WimaPlanData.h \
src/Wima/Geometry/WimaJoinedArea.h \
src/Wima/Geometry/WimaJoinedAreaData.h \
src/Wima/CircularSurveyComplexItem.h \
src/Wima/Geometry/PlanimetryCalculus.h \
src/Wima/Geometry/Circle.h \
src/Wima/Geometry/PolygonCalculus.h \
src/Wima/OptimisationTools.h \
src/Wima/Geometry/GeoUtilities.h \
......@@ -500,6 +502,7 @@ HEADERS += \
src/comm/ros_bridge/include/topic_subscriber.h \
src/comm/utilities.h
SOURCES += \
src/Wima/CircularSurvey.cc \
src/Wima/Snake/clipper/clipper.cpp \
src/Wima/Snake/snake.cpp \
src/Wima/Geometry/GeoPoint3D.cpp \
......@@ -548,9 +551,7 @@ SOURCES += \
src/Wima/Geometry/WimaMeasurementAreaData.cc \
src/Wima/Geometry/WimaJoinedArea.cc \
src/Wima/Geometry/WimaJoinedAreaData.cc \
src/Wima/CircularSurveyComplexItem.cc \
src/Wima/Geometry/PlanimetryCalculus.cc \
src/Wima/Geometry/Circle.cc \
src/Wima/OptimisationTools.cc \
src/Wima/Geometry/GeoUtilities.cc \
src/Wima/Geometry/PolygonCalculus.cc \
......
src/MissionManager/MissionController.cc
View file @ 7271b853
......@@ -31,7 +31,7 @@
#include "KML.h"
#include "QGCCorePlugin.h"
#include "src/Wima/CircularSurveyComplexItem.h"
#include "src/Wima/CircularSurvey.h"
#ifndef __mobile__
#include "MainWindow.h"
......@@ -544,7 +544,7 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
} else if (itemName == patternCorridorScanName) {
newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
} else if (itemName == _circularSurveyMissionItemName) {
newItem = new CircularSurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
newItem = new CircularSurvey(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
} else {
qWarning() << "Internal error: Unknown complex item:" << itemName;
return sequenceNumber;
......@@ -560,7 +560,7 @@ int MissionController::insertComplexMissionItemFromKMLOrSHP(QString itemName, QS
if (itemName == _surveyMissionItemName) {
newItem = new SurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems);
} else if (itemName == _circularSurveyMissionItemName) {
newItem = new CircularSurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems);
newItem = new CircularSurvey(_controllerVehicle, _flyView, file, _visualItems);
} else if (itemName == patternStructureScanName) {
newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, file, _visualItems);
} else if (itemName == patternCorridorScanName) {
......@@ -578,7 +578,7 @@ int MissionController::_insertComplexMissionItemWorker(ComplexMissionItem* compl
int sequenceNumber = _nextSequenceNumber();
bool surveyStyleItem = qobject_cast<SurveyComplexItem*>(complexItem)
|| qobject_cast<CorridorScanComplexItem*>(complexItem)
|| qobject_cast<CircularSurveyComplexItem*>(complexItem);
|| qobject_cast<CircularSurvey*>(complexItem);
if (surveyStyleItem) {
bool rollSupported = false;
......@@ -633,7 +633,7 @@ void MissionController::removeMissionItem(int index)
bool removeSurveyStyle = _visualItems->value<SurveyComplexItem*>(index)
|| _visualItems->value<CorridorScanComplexItem*>(index)
|| _visualItems->value<CircularSurveyComplexItem*>(index);
|| _visualItems->value<CircularSurvey*>(index);
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(index));
_deinitVisualItem(item);
......@@ -645,7 +645,7 @@ void MissionController::removeMissionItem(int index)
for (int i=1; i<_visualItems->count(); i++) {
if ( _visualItems->value<SurveyComplexItem*>(i)
|| _visualItems->value<CorridorScanComplexItem*>(i)
|| _visualItems->value<CircularSurveyComplexItem*>(i)) {
|| _visualItems->value<CircularSurvey*>(i)) {
foundSurvey = true;
break;
}
......@@ -905,9 +905,9 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec
nextSequenceNumber = corridorItem->lastSequenceNumber() + 1;
qCDebug(MissionControllerLog) << "Corridor Scan load complete: nextSequenceNumber" << nextSequenceNumber;
visualItems->append(corridorItem);
} else if (complexItemType == CircularSurveyComplexItem::jsonComplexItemTypeValue) {
} else if (complexItemType == CircularSurvey::CircularSurveyName) {
qCDebug(MissionControllerLog) << "Loading Circular Survey: nextSequenceNumber" << nextSequenceNumber;
CircularSurveyComplexItem* circularSurvey = new CircularSurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems);
CircularSurvey* circularSurvey = new CircularSurvey(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems);
if (!circularSurvey->load(itemObject, nextSequenceNumber++, errorString)) {
return false;
}
......
src/MissionManager/MissionItem.h
View file @ 7271b853
......@@ -26,7 +26,7 @@
#include "QmlObjectListModel.h"
class SurveyComplexItem;
class CircularSurveyComplexItem;
class CircularSurvey;
class SimpleMissionItem;
class MissionController;
#ifdef UNITTEST_BUILD
......@@ -153,7 +153,7 @@ private:
static const char* _jsonParam4Key;
friend class SurveyComplexItem;
friend class CircularSurveyComplexItem;
friend class CircularSurvey;
friend class SimpleMissionItem;
friend class MissionController;
#ifdef UNITTEST_BUILD
......
src/MissionManager/TransectStyleComplexItem.h
View file @ 7271b853
......@@ -9,147 +9,179 @@
#pragma once
#include "CameraCalc.h"
#include "ComplexMissionItem.h"
#include "MissionItem.h"
#include "SettingsFact.h"
#include "QGCLoggingCategory.h"
#include "QGCMapPolyline.h"
#include "QGCMapPolygon.h"
#include "CameraCalc.h"
#include "QGCMapPolyline.h"
#include "SettingsFact.h"
#include "TerrainQuery.h"
Q_DECLARE_LOGGING_CATEGORY(TransectStyleComplexItemLog)
class TransectStyleComplexItem : public ComplexMissionItem
{
class TransectStyleComplexItem : public ComplexMissionItem {
Q_OBJECT
public:
TransectStyleComplexItem(Vehicle* vehicle, bool flyView, QString settignsGroup, QObject* parent);
TransectStyleComplexItem(Vehicle *vehicle, bool flyView,
QString settignsGroup, QObject *parent);
Q_PROPERTY(QGCMapPolygon* surveyAreaPolygon READ surveyAreaPolygon CONSTANT)
Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT)
Q_PROPERTY(Fact* turnAroundDistance READ turnAroundDistance CONSTANT)
Q_PROPERTY(Fact* cameraTriggerInTurnAround READ cameraTriggerInTurnAround CONSTANT)
Q_PROPERTY(Fact* hoverAndCapture READ hoverAndCapture CONSTANT)
Q_PROPERTY(Fact* refly90Degrees READ refly90Degrees CONSTANT)
Q_PROPERTY(QGCMapPolygon *surveyAreaPolygon READ surveyAreaPolygon CONSTANT)
Q_PROPERTY(CameraCalc *cameraCalc READ cameraCalc CONSTANT)
Q_PROPERTY(Fact *turnAroundDistance READ turnAroundDistance CONSTANT)
Q_PROPERTY(
Fact *cameraTriggerInTurnAround READ cameraTriggerInTurnAround CONSTANT)
Q_PROPERTY(Fact *hoverAndCapture READ hoverAndCapture CONSTANT)
Q_PROPERTY(Fact *refly90Degrees READ refly90Degrees CONSTANT)
Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged)
Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged)
Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY
timeBetweenShotsChanged)
Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged)
Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT)
Q_PROPERTY(QVariantList visualTransectPoints READ visualTransectPoints NOTIFY visualTransectPointsChanged)
Q_PROPERTY(bool followTerrain READ followTerrain WRITE setFollowTerrain NOTIFY followTerrainChanged)
Q_PROPERTY(Fact* terrainAdjustTolerance READ terrainAdjustTolerance CONSTANT)
Q_PROPERTY(Fact* terrainAdjustMaxDescentRate READ terrainAdjustMaxDescentRate CONSTANT)
Q_PROPERTY(Fact* terrainAdjustMaxClimbRate READ terrainAdjustMaxClimbRate CONSTANT)
QGCMapPolygon* surveyAreaPolygon (void) { return &_surveyAreaPolygon; }
CameraCalc* cameraCalc (void) { return &_cameraCalc; }
Q_PROPERTY(QVariantList visualTransectPoints READ visualTransectPoints NOTIFY
visualTransectPointsChanged)
Q_PROPERTY(bool followTerrain READ followTerrain WRITE setFollowTerrain NOTIFY
followTerrainChanged)
Q_PROPERTY(Fact *terrainAdjustTolerance READ terrainAdjustTolerance CONSTANT)
Q_PROPERTY(Fact *terrainAdjustMaxDescentRate READ terrainAdjustMaxDescentRate
CONSTANT)
Q_PROPERTY(
Fact *terrainAdjustMaxClimbRate READ terrainAdjustMaxClimbRate CONSTANT)
QGCMapPolygon *surveyAreaPolygon(void) { return &_surveyAreaPolygon; }
CameraCalc *cameraCalc(void) { return &_cameraCalc; }
QVariantList visualTransectPoints(void) { return _visualTransectPoints; }
Fact* turnAroundDistance (void) { return &_turnAroundDistanceFact; }
Fact* cameraTriggerInTurnAround (void) { return &_cameraTriggerInTurnAroundFact; }
Fact* hoverAndCapture (void) { return &_hoverAndCaptureFact; }
Fact* refly90Degrees (void) { return &_refly90DegreesFact; }
Fact* terrainAdjustTolerance (void) { return &_terrainAdjustToleranceFact; }
Fact* terrainAdjustMaxDescentRate (void) { return &_terrainAdjustMaxDescentRateFact; }
Fact* terrainAdjustMaxClimbRate (void) { return &_terrainAdjustMaxClimbRateFact; }
const Fact* hoverAndCapture (void) const { return &_hoverAndCaptureFact; }
int cameraShots (void) const { return _cameraShots; }
double coveredArea (void) const;
bool hoverAndCaptureAllowed (void) const;
bool followTerrain (void) const { return _followTerrain; }
virtual double timeBetweenShots (void) { return 0; } // Most be overridden. Implementation here is needed for unit testing.
Fact *turnAroundDistance(void) { return &_turnAroundDistanceFact; }
Fact *cameraTriggerInTurnAround(void) {
return &_cameraTriggerInTurnAroundFact;
}
Fact *hoverAndCapture(void) { return &_hoverAndCaptureFact; }
Fact *refly90Degrees(void) { return &_refly90DegreesFact; }
Fact *terrainAdjustTolerance(void) { return &_terrainAdjustToleranceFact; }
Fact *terrainAdjustMaxDescentRate(void) {
return &_terrainAdjustMaxDescentRateFact;
}
Fact *terrainAdjustMaxClimbRate(void) {
return &_terrainAdjustMaxClimbRateFact;
}
const Fact *hoverAndCapture(void) const { return &_hoverAndCaptureFact; }
int cameraShots(void) const { return _cameraShots; }
double coveredArea(void) const;
bool hoverAndCaptureAllowed(void) const;
bool followTerrain(void) const { return _followTerrain; }
virtual double timeBetweenShots(void) {
return 0;
} // Most be overridden. Implementation here is needed for unit testing.
void setFollowTerrain(bool followTerrain);
double triggerDistance (void) const { return _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(); }
bool hoverAndCaptureEnabled (void) const { return hoverAndCapture()->rawValue().toBool(); }
bool triggerCamera (void) const { return triggerDistance() != 0; }
double triggerDistance(void) const {
return _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble();
}
bool hoverAndCaptureEnabled(void) const {
return hoverAndCapture()->rawValue().toBool();
}
bool triggerCamera(void) const { return triggerDistance() != 0; }
// Overrides from ComplexMissionItem
int lastSequenceNumber (void) const final;
QString mapVisualQML (void) const override = 0;
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) override = 0;
int lastSequenceNumber(void) const final;
QString mapVisualQML(void) const override = 0;
bool load(const QJsonObject &complexObject, int sequenceNumber,
QString &errorString) override = 0;
double complexDistance (void) const final { return _complexDistance; }
double greatestDistanceTo (const QGeoCoordinate &other) const final;
double complexDistance(void) const final { return _complexDistance; }
double greatestDistanceTo(const QGeoCoordinate &other) const final;
// Overrides from VisualMissionItem
void save (QJsonArray& planItems) override = 0;
bool specifiesCoordinate (void) const override = 0;
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) override = 0;
void applyNewAltitude (double newAltitude) override = 0;
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
bool specifiesAltitudeOnly (void) const final { return false; }
QGeoCoordinate coordinate (void) const final { return _coordinate; }
QGeoCoordinate exitCoordinate (void) const final { return _exitCoordinate; }
int sequenceNumber (void) const final { return _sequenceNumber; }
double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalYaw (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalPitch (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void setMissionFlightStatus (const MissionController::MissionFlightStatus_t& missionFlightStatus) final;
bool readyForSave (void) const override;
QString commandDescription (void) const override { return tr("Transect"); }
QString commandName (void) const override { return tr("Transect"); }
QString abbreviation (void) const override { return tr("T"); }
bool coordinateHasRelativeAltitude (void) const final;
bool exitCoordinateHasRelativeAltitude (void) const final;
bool exitCoordinateSameAsEntry (void) const final { return false; }
void setDirty (bool dirty) final;
void setCoordinate (const QGeoCoordinate& coordinate) final { Q_UNUSED(coordinate); }
void setSequenceNumber (int sequenceNumber) final;
static const char* turnAroundDistanceName;
static const char* turnAroundDistanceMultiRotorName;
static const char* cameraTriggerInTurnAroundName;
static const char* hoverAndCaptureName;
static const char* refly90DegreesName;
static const char* terrainAdjustToleranceName;
static const char* terrainAdjustMaxClimbRateName;
static const char* terrainAdjustMaxDescentRateName;
void save(QJsonArray &planItems) override = 0;
bool specifiesCoordinate(void) const override = 0;
void appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) override = 0;
void applyNewAltitude(double newAltitude) override = 0;
bool dirty(void) const final { return _dirty; }
bool isSimpleItem(void) const final { return false; }
bool isStandaloneCoordinate(void) const final { return false; }
bool specifiesAltitudeOnly(void) const final { return false; }
QGeoCoordinate coordinate(void) const final { return _coordinate; }
QGeoCoordinate exitCoordinate(void) const final { return _exitCoordinate; }
int sequenceNumber(void) const final { return _sequenceNumber; }
double specifiedFlightSpeed(void) final {
return std::numeric_limits<double>::quiet_NaN();
}
double specifiedGimbalYaw(void) final {
return std::numeric_limits<double>::quiet_NaN();
}
double specifiedGimbalPitch(void) final {
return std::numeric_limits<double>::quiet_NaN();
}
void setMissionFlightStatus(const MissionController::MissionFlightStatus_t
&missionFlightStatus) final;
bool readyForSave(void) const override;
QString commandDescription(void) const override { return tr("Transect"); }
QString commandName(void) const override { return tr("Transect"); }
QString abbreviation(void) const override { return tr("T"); }
bool coordinateHasRelativeAltitude(void) const final;
bool exitCoordinateHasRelativeAltitude(void) const final;
bool exitCoordinateSameAsEntry(void) const final { return false; }
void setDirty(bool dirty) final;
void setCoordinate(const QGeoCoordinate &coordinate) final {
Q_UNUSED(coordinate);
}
void setSequenceNumber(int sequenceNumber) final;
static const char *turnAroundDistanceName;
static const char *turnAroundDistanceMultiRotorName;
static const char *cameraTriggerInTurnAroundName;
static const char *hoverAndCaptureName;
static const char *refly90DegreesName;
static const char *terrainAdjustToleranceName;
static const char *terrainAdjustMaxClimbRateName;
static const char *terrainAdjustMaxDescentRateName;
signals:
void cameraShotsChanged (void);
void timeBetweenShotsChanged (void);
void visualTransectPointsChanged (void);
void coveredAreaChanged (void);
void followTerrainChanged (bool followTerrain);
void missionItemReady (void);
void cameraShotsChanged(void);
void timeBetweenShotsChanged(void);
void visualTransectPointsChanged(void);
void coveredAreaChanged(void);
void followTerrainChanged(bool followTerrain);
void missionItemReady(void);
protected slots:
void _setDirty (void);
void _setIfDirty (bool dirty);
void _updateCoordinateAltitudes (void);
void _polyPathTerrainData (bool success, const QList<TerrainPathQuery::PathHeightInfo_t>& rgPathHeightInfo);
void _rebuildTransects (void);
void _updateTransectAltitude (void);
void _clearLoadedMissionItems (void);
void _setDirty(void);
void _setIfDirty(bool dirty);
void _updateCoordinateAltitudes(void);
void _polyPathTerrainData(
bool success,
const QList<TerrainPathQuery::PathHeightInfo_t> &rgPathHeightInfo);
void _rebuildTransects(void);
void _updateTransectAltitude(void);
void _clearLoadedMissionItems(void);
protected:
virtual void _rebuildTransectsPhase1 (void) = 0; ///< Rebuilds the _transects array
virtual void _recalcComplexDistance (void) = 0;
virtual void _recalcCameraShots (void) = 0;
void _save (QJsonObject& saveObject);
bool _load (const QJsonObject& complexObject, QString& errorString);
void _setExitCoordinate (const QGeoCoordinate& coordinate);
void _setCameraShots (int cameraShots);
double _triggerDistance (void) const;
bool _hasTurnaround (void) const;
double _turnaroundDistance (void) const;
virtual void
_rebuildTransectsPhase1(void) = 0; ///< Rebuilds the _transects array
virtual void _recalcComplexDistance(void) = 0;
virtual void _recalcCameraShots(void) = 0;
void _save(QJsonObject &saveObject);
bool _load(const QJsonObject &complexObject, QString &errorString);
void _setExitCoordinate(const QGeoCoordinate &coordinate);
void _setCameraShots(int cameraShots);
double _triggerDistance(void) const;
bool _hasTurnaround(void) const;
double _turnaroundDistance(void) const;
int _sequenceNumber;
QGeoCoordinate _coordinate;
......@@ -158,7 +190,8 @@ protected:
enum CoordType {
CoordTypeInterior, ///< Interior waypoint for flight path only
CoordTypeInteriorHoverTrigger, ///< Interior waypoint for hover and capture trigger
CoordTypeInteriorHoverTrigger, ///< Interior waypoint for hover and capture
///< trigger
CoordTypeInteriorTerrainAdded, ///< Interior waypoint added for terrain
CoordTypeSurveyEdge, ///< Waypoint at edge of survey polygon
CoordTypeTurnaround ///< Waypoint outside of survey polygon for turnaround
......@@ -170,9 +203,10 @@ protected:
} CoordInfo_t;
QVariantList _visualTransectPoints;
QList<QList<CoordInfo_t>> _transects;
using Transects = QList<QList<CoordInfo_t>>;
Transects _transects;
QList<QList<TerrainPathQuery::PathHeightInfo_t>> _transectsPathHeightInfo;
TerrainPolyPathQuery* _terrainPolyPathQuery;
TerrainPolyPathQuery *_terrainPolyPathQuery;
QTimer _terrainQueryTimer;
bool _ignoreRecalc;
......@@ -183,10 +217,13 @@ protected:
CameraCalc _cameraCalc;
bool _followTerrain;
QObject* _loadedMissionItemsParent; ///< Parent for all items in _loadedMissionItems for simpler delete
QList<MissionItem*> _loadedMissionItems; ///< Mission items loaded from plan file
QObject
*_loadedMissionItemsParent; ///< Parent for all items in
///< _loadedMissionItems for simpler delete
QList<MissionItem *>
_loadedMissionItems; ///< Mission items loaded from plan file
QMap<QString, FactMetaData*> _metaDataMap;
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _turnAroundDistanceFact;
SettingsFact _cameraTriggerInTurnAroundFact;
......@@ -196,30 +233,33 @@ protected:
SettingsFact _terrainAdjustMaxClimbRateFact;
SettingsFact _terrainAdjustMaxDescentRateFact;
static const char* _jsonCameraCalcKey;
static const char* _jsonTransectStyleComplexItemKey;
static const char* _jsonVisualTransectPointsKey;
static const char* _jsonItemsKey;
static const char* _jsonFollowTerrainKey;
static const char* _jsonCameraShotsKey;
static const char *_jsonCameraCalcKey;
static const char *_jsonTransectStyleComplexItemKey;
static const char *_jsonVisualTransectPointsKey;
static const char *_jsonItemsKey;
static const char *_jsonFollowTerrainKey;
static const char *_jsonCameraShotsKey;
static const int _terrainQueryTimeoutMsecs;
private slots:
void _reallyQueryTransectsPathHeightInfo(void);
void _followTerrainChanged (bool followTerrain);
void _followTerrainChanged(bool followTerrain);
protected:
bool _transectsDirty;
private:
void _queryTransectsPathHeightInfo (void);
void _adjustTransectsForTerrain (void);
void _addInterstitialTerrainPoints (QList<CoordInfo_t>& transect, const QList<TerrainPathQuery::PathHeightInfo_t>& transectPathHeightInfo);
void _adjustForMaxRates (QList<CoordInfo_t>& transect);
void _adjustForTolerance (QList<CoordInfo_t>& transect);
double _altitudeBetweenCoords (const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord, double percentTowardsTo);
int _maxPathHeight (const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo, int fromIndex, int toIndex, double& maxHeight);
void _queryTransectsPathHeightInfo(void);
void _adjustTransectsForTerrain(void);
void _addInterstitialTerrainPoints(
QList<CoordInfo_t> &transect,
const QList<TerrainPathQuery::PathHeightInfo_t> &transectPathHeightInfo);
void _adjustForMaxRates(QList<CoordInfo_t> &transect);
void _adjustForTolerance(QList<CoordInfo_t> &transect);
double _altitudeBetweenCoords(const QGeoCoordinate &fromCoord,
const QGeoCoordinate &toCoord,
double percentTowardsTo);
int _maxPathHeight(const TerrainPathQuery::PathHeightInfo_t &pathHeightInfo,
int fromIndex, int toIndex, double &maxHeight);
};
src/PlanView/CircularSurveyItemEditor.qml
View file @ 7271b853
......@@ -137,11 +137,6 @@ Rectangle {
spacing: _margin
visible: transectsHeader.checked
FactCheckBox {
text: qsTr("Fixed Direction")
fact: missionItem.fixedDirection
}
QGCCheckBox {
id: relAlt
text: qsTr("Relative altitude")
......
src/Wima/CircularSurvey.cc 0 → 100644
View file @ 7271b853
#include "CircularSurvey.h"
#include <QtConcurrentRun>
#include "JsonHelper.h"
#include "QGCApplication.h"
#include <chrono>
#include "clipper/clipper.hpp"
#include "snake.h"
#define CLIPPER_SCALE 10000
template <int k> ClipperLib::cInt get(ClipperLib::IntPoint &p);
#include "Geometry/GenericCircle.h"
#include "Geometry/GeoUtilities.h"
#include "Geometry/PlanimetryCalculus.h"
#include "Geometry/PolygonCalculus.h"
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
template <> ClipperLib::cInt get<0>(ClipperLib::IntPoint &p) { return p.X; }
template <> ClipperLib::cInt get<1>(ClipperLib::IntPoint &p) { return p.Y; }
template <class Functor> class CommandRAII {
public:
CommandRAII(Functor f) : fun(f) {}
~CommandRAII() { fun(); }
private:
Functor fun;
};
const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::deltaRName = "DeltaR";
const char *CircularSurvey::deltaAlphaName = "DeltaAlpha";
const char *CircularSurvey::transectMinLengthName = "TransectMinLength";
const char *CircularSurvey::reverseName = "Reverse";
const char *CircularSurvey::maxWaypointsName = "MaxWaypoints";
const char *CircularSurvey::CircularSurveyName = "CircularSurvey";
const char *CircularSurvey::refPointLatitudeName = "ReferencePointLat";
const char *CircularSurvey::refPointLongitudeName = "ReferencePointLong";
const char *CircularSurvey::refPointAltitudeName = "ReferencePointAlt";
CircularSurvey::CircularSurvey(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]),
_minLength(settingsGroup, _metaDataMap[transectMinLengthName]),
_reverse(settingsGroup, _metaDataMap[reverseName]),
_maxWaypoints(settingsGroup, _metaDataMap[maxWaypointsName]),
_isInitialized(false), _calculating(false), _cancle(false) {
Q_UNUSED(kmlOrShpFile)
_editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";
// Defer update if facts or ref. changes.
connect(&_deltaR, &Fact::valueChanged, this, &CircularSurvey::_deferUpdate);
connect(&_deltaAlpha, &Fact::valueChanged, this,
&CircularSurvey::_deferUpdate);
connect(&_minLength, &Fact::valueChanged, this,
&CircularSurvey::_deferUpdate);
connect(&_maxWaypoints, &Fact::valueChanged, [this] {
this->CircularSurvey::_deferUpdate();
qWarning() << "max waypoints implementaion missing";
});
connect(&_reverse, &Fact::valueChanged, [this] {
this->CircularSurvey::_deferUpdate();
qWarning() << "reverse implementaion missing";
});
connect(this, &CircularSurvey::refPointChanged, this,
&CircularSurvey::_deferUpdate);
connect(&this->_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this,
&CircularSurvey::_deferUpdate);
// Setup Timer.
_timer.setSingleShot(true);
connect(&_timer, &QTimer::timeout, [this] { this->_rebuildTransects(); });
// Future watcher.
connect(&_watcher, &Watcher::finished, [this] {
this->_calculating = false;
emit calculatingChanged();
if (!_cancle) {
this->_transectsDirty = false;
} else {
_cancle = false;
}
this->_rebuildTransects();
});
}
void CircularSurvey::resetReference() {
setRefPoint(_surveyAreaPolygon.center());
}
void CircularSurvey::setRefPoint(const QGeoCoordinate &refPt) {
if (refPt != _referencePoint) {
_referencePoint = refPt;
emit refPointChanged();
}
}
void CircularSurvey::setIsInitialized(bool isInitialized) {
if (isInitialized != _isInitialized) {
_isInitialized = isInitialized;
emit isInitializedChanged();
}
}
QGeoCoordinate CircularSurvey::refPoint() const { return _referencePoint; }
Fact *CircularSurvey::deltaR() { return &_deltaR; }
Fact *CircularSurvey::deltaAlpha() { return &_deltaAlpha; }
bool CircularSurvey::isInitialized() { return _isInitialized; }
bool CircularSurvey::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},
{deltaRName, QJsonValue::Double, true},
{deltaAlphaName, QJsonValue::Double, true},
{transectMinLengthName, QJsonValue::Double, true},
{reverseName, QJsonValue::Bool, true},
{refPointLatitudeName, QJsonValue::Double, true},
{refPointLongitudeName, QJsonValue::Double, true},
{refPointAltitudeName, 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 != CircularSurveyName) {
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[deltaRName].toDouble());
_deltaAlpha.setRawValue(complexObject[deltaAlphaName].toDouble());
_minLength.setRawValue(complexObject[transectMinLengthName].toDouble());
_referencePoint.setLongitude(complexObject[refPointLongitudeName].toDouble());
_referencePoint.setLatitude(complexObject[refPointLatitudeName].toDouble());
_referencePoint.setAltitude(complexObject[refPointAltitudeName].toDouble());
_reverse.setRawValue(complexObject[reverseName].toBool());
setIsInitialized(true);
_ignoreRecalc = false;
_recalcComplexDistance();
if (_cameraShots == 0) {
// Shot count was possibly not available from plan file
_recalcCameraShots();
}
return true;
}
QString CircularSurvey::mapVisualQML() const {
return QStringLiteral("CircularSurveyMapVisual.qml");
}
void CircularSurvey::save(QJsonArray &planItems) {
QJsonObject saveObject;
_save(saveObject);
saveObject[JsonHelper::jsonVersionKey] = 1;
saveObject[VisualMissionItem::jsonTypeKey] =
VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = CircularSurveyName;
saveObject[deltaRName] = _deltaR.rawValue().toDouble();
saveObject[deltaAlphaName] = _deltaAlpha.rawValue().toDouble();
saveObject[transectMinLengthName] = _minLength.rawValue().toDouble();
saveObject[reverseName] = _reverse.rawValue().toBool();
saveObject[refPointLongitudeName] = _referencePoint.longitude();
saveObject[refPointLatitudeName] = _referencePoint.latitude();
saveObject[refPointAltitudeName] = _referencePoint.altitude();
// Polygon shape
_surveyAreaPolygon.saveToJson(saveObject);
planItems.append(saveObject);
}
bool CircularSurvey::specifiesCoordinate() const { return true; }
void CircularSurvey::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 CircularSurvey::_appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty)
return;
int seqNum = _sequenceNumber;
for (const MissionItem *loadedMissionItem : _loadedMissionItems) {
MissionItem *item = new MissionItem(*loadedMissionItem, missionItemParent);
item->setSequenceNumber(seqNum++);
items.append(item);
}
}
void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
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 CircularSurvey::applyNewAltitude(double newAltitude) {
_cameraCalc.valueSetIsDistance()->setRawValue(true);
_cameraCalc.distanceToSurface()->setRawValue(newAltitude);
_cameraCalc.setDistanceToSurfaceRelative(true);
}
double CircularSurvey::timeBetweenShots() { return 1; }
QString CircularSurvey::commandDescription() const {
return tr("Circular Survey");
}
QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString CircularSurvey::abbreviation() const { return tr("C.S."); }
bool CircularSurvey::readyForSave() const {
return TransectStyleComplexItem::readyForSave() && !_transectsDirty;
}
double CircularSurvey::additionalTimeDelay() const { return 0; }
void CircularSurvey::_rebuildTransectsPhase1(void) {
if (_calculating)
return;
if (!_transectsDirty) {
auto pTransects = _watcher.result();
if (pTransects) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning()
<< "CircularSurvey::_rebuildTransectsPhase1(): storing transects.";
#endif
// If the transects are getting rebuilt then any previously loaded
// mission items are now invalid.
if (_loadedMissionItemsParent) {
_loadedMissionItems.clear();
_loadedMissionItemsParent->deleteLater();
_loadedMissionItemsParent = nullptr;
}
// Store new transects;
_transects = *pTransects;
}
} else {
_transects.clear();
// Check preconitions
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning()
<< "CircularSurvey::_rebuildTransectsPhase1(): checking preconditions.";
#endif
if (_isInitialized && _surveyAreaPolygon.count() >= 3 &&
_deltaAlpha.rawValue() <= _deltaAlpha.rawMax() &&
_deltaAlpha.rawValue() >= _deltaAlpha.rawMin() &&
_deltaR.rawValue() <= _deltaR.rawMax() &&
_deltaR.rawValue() >= _deltaR.rawMin()) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning()
<< "CircularSurvey::_rebuildTransectsPhase1(): preconditions ok.";
#endif
using namespace boost::units;
_calculating = true;
emit calculatingChanged();
// Copy input.
const auto &polygon = this->_surveyAreaPolygon.coordinateList();
const auto &origin = this->_referencePoint;
const snake::Length deltaR(this->_deltaR.rawValue().toDouble() *
si::meter);
const snake::Angle deltaAlpha(this->_deltaAlpha.rawValue().toDouble() *
degree::degree);
const snake::Length minLength(this->_minLength.rawValue().toDouble() *
si::meter);
const auto maxWaypoints(this->_maxWaypoints.rawValue().toUInt());
auto future = QtConcurrent::run([polygon, origin, deltaAlpha, minLength,
maxWaypoints, deltaR] {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): calculation "
"started.";
#endif
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto start = std::chrono::high_resolution_clock::now();
auto onExit = [&start] {
qWarning() << "CircularSurvey: concurrent update execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
};
CommandRAII<decltype(onExit)> commandRAII(onExit);
#endif
// Convert geo polygon to ENU polygon.
snake::BoostPolygon polygonENU;
snake::BoostPoint originENU{0, 0};
snake::areaToEnu(origin, polygon, polygonENU);
// Check validity.
if (!bg::is_valid(polygonENU)) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
"invalid polygon.";
#endif
return PtrTransects();
} else {
// Calculate polygon distances and angles.
std::vector<snake::Length> distances;
distances.reserve(polygonENU.outer().size());
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);
}
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 =
(*std::max_element(distances.begin(),
distances.end())); // maximal circle radius
// Scale parameters and coordinates.
const auto rMinScaled =
ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
const auto deltaRScaled =
ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
auto originScaled = ClipperLib::IntPoint{
ClipperLib::cInt(std::round(originENU.get<0>())),
ClipperLib::cInt(std::round(originENU.get<1>()))};
// Generate circle sectors.
auto rScaled = rMinScaled;
const auto nTran = long(std::floor(((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() << "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);
rScaled += deltaRScaled;
}
// Clip sectors to polygonENU.
ClipperLib::Path polygonClipper;
auto &outer = polygonENU.outer();
polygonClipper.reserve(outer.size() - 1);
for (auto it = outer.begin(); it < outer.end() - 1; ++it) {
auto x = ClipperLib::cInt(std::round(it->get<0>() * CLIPPER_SCALE));
auto y = ClipperLib::cInt(std::round(it->get<1>() * CLIPPER_SCALE));
polygonClipper.push_back(ClipperLib::IntPoint{x, y});
}
ClipperLib::Clipper clipper;
clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
ClipperLib::PolyTree transectsClipper;
clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Extract transects from PolyTree and convert them to
// BoostLineString
snake::Transects transectsENU;
for (const auto &child : transectsClipper.Childs) {
snake::BoostLineString transect;
transect.reserve(child->Contour.size());
for (const auto &vertex : child->Contour) {
auto x = static_cast<double>(vertex.X) / CLIPPER_SCALE;
auto y = static_cast<double>(vertex.Y) / CLIPPER_SCALE;
transect.push_back(snake::BoostPoint(x, y));
}
if (bg::length(transect) >= minLength.value()) {
transectsENU.push_back(transect);
}
}
if (transectsENU.size() == 0) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
"not able to generate transects.";
#endif
return PtrTransects();
}
// Route transects;
snake::Transects transectsRouted;
snake::Route route;
std::string errorString;
bool success = snake::route(polygonENU, transectsENU, transectsRouted,
route, errorString);
if (!success) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
"routing failed.";
#endif
return PtrTransects();
}
QList<CoordInfo_t> transectList;
transectList.reserve(route.size());
for (const auto &vertex : route) {
QGeoCoordinate c;
snake::fromENU(origin, vertex, c);
CoordInfo_t coordinfo = {c, CoordTypeInterior};
transectList.append(coordinfo);
}
PtrTransects transects(new Transects());
transects->append(transectList);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
"concurrent update success.";
#endif
return transects;
}
}); // QtConcurrent::run()
_watcher.setFuture(future);
}
#ifdef DEBUG_CIRCULAR_SURVEY
else {
qWarning()
<< "CircularSurvey::_rebuildTransectsPhase1(): preconditions failed.";
}
#endif
}
}
void CircularSurvey::_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 CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
void CircularSurvey::_deferUpdate() {
if (!_calculating) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CircularSurvey::_deferUpdate(): defer update.";
#endif
_transectsDirty = true;
if (_timer.isActive()) {
_timer.stop();
}
_timer.start(100 /*ms*/);
} else {
_cancle = true;
}
}
Fact *CircularSurvey::transectMinLength() { return &_minLength; }
Fact *CircularSurvey::reverse() { return &_reverse; }
Fact *CircularSurvey::maxWaypoints() { return &_maxWaypoints; }
bool CircularSurvey::calculating() { return _calculating; }
/*!
\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.h 0 → 100644
View file @ 7271b853
#pragma once
#include <QFutureWatcher>
#include <QVector>
#include "SettingsFact.h"
#include "TransectStyleComplexItem.h"
class CircularSurvey : public TransectStyleComplexItem {
Q_OBJECT
public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default
/// polygon
CircularSurvey(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile,
QObject *parent);
Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY
refPointChanged)
Q_PROPERTY(Fact *deltaR READ deltaR CONSTANT)
Q_PROPERTY(Fact *deltaAlpha READ deltaAlpha CONSTANT)
Q_PROPERTY(Fact *transectMinLength READ transectMinLength CONSTANT)
Q_PROPERTY(Fact *reverse READ reverse CONSTANT)
Q_PROPERTY(Fact *maxWaypoints READ maxWaypoints CONSTANT)
Q_PROPERTY(bool isInitialized READ isInitialized WRITE setIsInitialized NOTIFY
isInitializedChanged)
Q_PROPERTY(bool calculating READ calculating NOTIFY calculatingChanged)
Q_INVOKABLE void resetReference(void);
// Property setters
void setRefPoint(const QGeoCoordinate &refPt);
// Set this to true if survey was automatically generated, prevents
// initialisation from gui.
void setIsInitialized(bool isInitialized);
// Property getters
QGeoCoordinate refPoint() const;
Fact *deltaR();
Fact *deltaAlpha();
Fact *transectMinLength();
Fact *reverse();
Fact *maxWaypoints();
bool calculating();
// Is true if survey was automatically generated, prevents initialisation from
// gui.
bool isInitialized();
// Overrides from ComplexMissionItem
bool load(const QJsonObject &complexObject, int sequenceNumber,
QString &errorString) final;
QString mapVisualQML(void) const final;
// Overrides from TransectStyleComplexItem
void save(QJsonArray &planItems) final;
bool specifiesCoordinate(void) const final;
void appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) final;
void applyNewAltitude(double newAltitude) final;
double timeBetweenShots(void) final;
// Overrides from VisualMissionionItem
QString commandDescription(void) const final;
QString commandName(void) const final;
QString abbreviation(void) const final;
bool readyForSave(void) const final;
double additionalTimeDelay(void) const final;
static const char *settingsGroup;
static const char *deltaRName;
static const char *deltaAlphaName;
static const char *transectMinLengthName;
static const char *reverseName;
static const char *maxWaypointsName;
static const char *CircularSurveyName;
static const char *refPointLongitudeName;
static const char *refPointLatitudeName;
static const char *refPointAltitudeName;
signals:
void refPointChanged();
void isInitializedChanged();
void calculatingChanged();
private slots:
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1(void) final;
void _recalcComplexDistance(void) final;
void _recalcCameraShots(void) final;
void _deferUpdate();
private:
void _appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
// center of the circular lanes, e.g. base station
QGeoCoordinate _referencePoint;
QMap<QString, FactMetaData *> _metaDataMap;
// distance between two neighbour circles
SettingsFact _deltaR;
// angle discretisation of the circles
SettingsFact _deltaAlpha;
// minimal transect lenght, transects are rejected if they are shorter than
// this value
SettingsFact _minLength;
// reverses the _transects path
SettingsFact _reverse;
// the maximum number of waypoints _transects (TransectStyleComplexItem) can
// contain (to avoid performance hits)
SettingsFact _maxWaypoints;
// Timer to defer recalc
QTimer _timer;
// indicates if the polygon and refpoint etc. are initialized, prevents
// reinitialisation from gui and execution of _rebuildTransectsPhase1 during
// init from gui
bool _isInitialized;
using PtrTransects = std::shared_ptr<Transects>;
using Watcher = QFutureWatcher<PtrTransects>;
Watcher _watcher;
bool _calculating;
bool _cancle;
};
src/Wima/CircularSurveyComplexItem.cc deleted 100644 → 0
View file @ a22db7f4
#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/CircularSurveyComplexItem.h deleted 100644 → 0
View file @ a22db7f4
# pragma once
#include "TransectStyleComplexItem.h"
#include "Geometry/PolygonCalculus.h"
#include "Geometry/PlanimetryCalculus.h"
#include "Geometry/GeoUtilities.h"
#include "QVector"
#include "Geometry/Circle.h"
#include "SettingsFact.h"
class CircularSurveyComplexItem :public TransectStyleComplexItem
{
Q_OBJECT
public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default polygon
CircularSurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent);
Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY refPointChanged)
Q_PROPERTY(Fact* deltaR READ deltaR CONSTANT)
Q_PROPERTY(Fact* deltaAlpha READ deltaAlpha CONSTANT)
Q_PROPERTY(Fact* transectMinLength READ transectMinLength CONSTANT)
Q_PROPERTY(Fact* fixedDirection READ fixedDirection CONSTANT)
Q_PROPERTY(Fact* reverse READ reverse CONSTANT)
Q_PROPERTY(Fact* maxWaypoints READ maxWaypoints CONSTANT)
Q_PROPERTY(bool isInitialized READ isInitialized WRITE setIsInitialized NOTIFY isInitializedChanged)
Q_INVOKABLE void resetReference(void);
Q_INVOKABLE void comprehensiveUpdate(void); // triggers a slow recalculation of the transects
// Property setters
void setRefPoint(const QGeoCoordinate &refPt);
// Set this to true if survey was automatically generated, prevents initialisation from gui.
void setIsInitialized(bool isInitialized);
// Property getters
QGeoCoordinate refPoint() const;
Fact *deltaR();
Fact *deltaAlpha();
Fact *transectMinLength();
Fact *fixedDirection();
Fact *reverse();
Fact *maxWaypoints();
// Is true if survey was automatically generated, prevents initialisation from gui.
bool isInitialized();
bool referencePointBeingChanged(); // returns true if the referencepoint is being changed (dragged by user)
// Overrides from ComplexMissionItem
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final;
QString mapVisualQML (void) const final { return QStringLiteral("CircularSurveyMapVisual.qml"); }
// Overrides from TransectStyleComplexItem
void save (QJsonArray& planItems) final;
bool specifiesCoordinate (void) const final { return true; }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
double timeBetweenShots (void) final;
// Overrides from VisualMissionionItem
QString commandDescription (void) const final { return tr("Circular Survey"); }
QString commandName (void) const final { return tr("Circular Survey"); }
QString abbreviation (void) const final { return tr("C.S."); }
bool readyForSave (void) const final;
double additionalTimeDelay (void) const final;
static const char* settingsGroup;
static const char* deltaRName;
static const char* deltaAlphaName;
static const char* transectMinLengthName;
static const char* fixedDirectionName;
static const char* reverseName;
static const char* maxWaypointsName;
static const char* jsonComplexItemTypeValue;
static const char* jsonDeltaRKey;
static const char* jsonDeltaAlphaKey;
static const char* jsonTransectMinLengthKey;
static const char* jsonfixedDirectionKey;
static const char* jsonReverseKey;
static const char* jsonReferencePointLongKey;
static const char* jsonReferencePointLatKey;
static const char* jsonReferencePointAltKey;
static const long triggerTime = 50; // trigger time (ms) for _triggerSlowRecalcTimer
signals:
void refPointChanged();
void isInitializedChanged();
private slots:
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1 (void) final; // calls _rebuildTransectsFast or _rebuildTransectsSlow depending on _fastRecalc
void _rebuildTransectsFast (void);
void _rebuildTransectsSlow (void); // the slow version of _rebuildTransectsFast which properly connects the _transects
void _triggerSlowRecalc (void);
bool _generateTransectPath (QVector<QVector<QPointF>> &transectPath, const QPolygonF &surveyPolygon);
bool _rebuildTransectsInputCheck(QPolygonF &poly);
void _rebuildTransectsToGeo (const QVector<QPointF> &path, const QGeoCoordinate &reference);
void _recalcComplexDistance (void) final;
void _recalcCameraShots (void) final;
void _reverseTransects (void);
bool _shortestPath (const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &shortestPath);
signals:
private:
void _appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent);
QGeoCoordinate _referencePoint; // center of the circular lanes, e.g. base station
QMap<QString, FactMetaData*> _metaDataMap;
SettingsFact _deltaR; // distance between two neighbour circles
SettingsFact _deltaAlpha; // angle discretisation of the circles
SettingsFact _transectMinLength; // minimal transect lenght, transects are rejected if they are shorter than this value
SettingsFact _fixedDirection; // bool value, determining if transects have fixed direction or not
SettingsFact _reverse; // reverses the _transects path
SettingsFact _maxWaypoints; // the maximum number of waypoints _transects (TransectStyleComplexItem) can contain (to avoid performance hits)
QTimer _triggerSlowRecalcTimer;
bool _isInitialized; // indicates if the polygon and refpoint etc. are initialized, prevents reinitialisation from gui and execution of _rebuildTransectsPhase1 during init from gui
bool _reverseOnly; // if this is true _rebuildTransectsPhase1() will reverse the path only, _rebuildTransectsPhase1() resets _reverseOnly
bool _doFastRecalc; // fast recalc of transects if set, see _rebuildTransectsPhase1 for furhter explanation
};
src/Wima/Geometry/Circle.cc deleted 100644 → 0
View file @ a22db7f4
#include "Circle.h"
Circle::Circle(QObject *parent)
: QObject (parent)
, _circleRadius(0)
, _circleOrigin(QPointF(0,0))
{
}
Circle::Circle(double radius, QObject *parent)
: QObject (parent)
, _circleRadius(radius >= 0 ? radius : 0)
, _circleOrigin(QPointF(0,0))
{
}
Circle::Circle(double radius, QPointF origin, QObject *parent)
: QObject (parent)
, _circleRadius(radius >= 0 ? radius : 0)
, _circleOrigin(origin)
{
}
Circle::Circle(const Circle &other, QObject *parent)
: QObject (parent)
{
*this = other;
}
Circle &Circle::operator=(const Circle &other)
{
this->setRadius(other.radius());
this->setOrigin(other.origin());
return *this;
}
/*!
* \fn void Circle::setRadius(double radius)
* Sets the radius of the circle to \a radius
*/
void Circle::setRadius(double radius)
{
if ( radius >= 0 && !qFuzzyCompare(_circleRadius, radius) ) {
_circleRadius = radius;
emit radiusChanged(_circleRadius);
}
}
/*!
* \fn void Circle::setOrigin(const QPointF &origin)
* Sets the origin of the circle to \a origin
*
* \sa QPointF
*/
void Circle::setOrigin(const QPointF &origin)
{
if (origin != _circleOrigin) {
_circleOrigin = origin;
emit originChanged(_circleOrigin);
}
}
/*!
* \fn double Circle::radius() const
* Returns the radius of the circle.
*/
double Circle::radius() const
{
return _circleRadius;
}
/*!
* \fn QPointF Circle::origin() const
* Returns the origin of the circle.
*
* \sa QPointF
*/
QPointF Circle::origin() const
{
return _circleOrigin;
}
/*!
* \fn QVector<QPointF> Circle::approximate(int numberOfCorners)
* Returns a polygon with \a numberOfCorners corners which approximates the circle.
*
* \sa QPointF
*/
QVector<QPointF> Circle::approximate(int numberOfCorners) const
{
if ( numberOfCorners < 3)
return QVector<QPointF>();
return approximateSektor(numberOfCorners+1, 0, 2*M_PI);
}
QVector<QPointF> Circle::approximate(double angleDiscretisation) const
{
return approximateSektor(angleDiscretisation, 0, 2*M_PI);
}
QVector<QPointF> Circle::approximateSektor(int numberOfCorners, double alpha1, double alpha2) const
{
if ( numberOfCorners < 2) {
qWarning("numberOfCorners < 2");
return QVector<QPointF>();
}
return approximateSektor(PlanimetryCalculus::truncateAngle(alpha2-alpha1)/double(numberOfCorners-1), alpha1, alpha2);
}
QVector<QPointF> Circle::approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const
{
using namespace PlanimetryCalculus;
// check if input is valid
if ( qFuzzyCompare(alpha1, alpha2) )
return QVector<QPointF>();
alpha1 = truncateAngle(alpha1);
alpha2 = truncateAngle(alpha2);
double deltaAlpha = fabs(alpha1 - alpha2);
if (signum(angleDiscretisation*(alpha1 - alpha2)) == 1) {
deltaAlpha = 2*M_PI - deltaAlpha;
}
// check if input is valid
if ( qFuzzyIsNull(angleDiscretisation))
return QVector<QPointF>();
QVector<QPointF> sector;
double currentAngle = alpha1;
// how many nodes?
int j = floor(fabs(deltaAlpha/angleDiscretisation));
// rotate the vertex j+1 times add the origin and append to the sector.
do {
sector.append(toCoordinate(currentAngle));
currentAngle = truncateAngle(currentAngle + angleDiscretisation);
}while(j--);
// append last point if necessarry
QPointF vertex = toCoordinate(alpha2);
if ( !qFuzzyIsNull(PlanimetryCalculus::distance(sector.first(), vertex))
&& !qFuzzyIsNull(PlanimetryCalculus::distance(sector.last(), vertex )) ){
sector.append(vertex);
}
return sector;
}
/*!
* \fn void Circle::toCoordinate(QPointF &coordinate, double alpha) const
* Calculates the coordinates of a point on the circle with angle \a alpha.
* Stores the result in \a coordiante.
*
* \sa QPointF
*/
void Circle::toCoordinate(QPointF &coordinate, double alpha) const
{
using namespace PlanimetryCalculus;
coordinate = QPointF(_circleRadius,0);
rotateReference(coordinate, alpha);
coordinate += _circleOrigin;
}
/*!
* \overload QPointF Circle::toCoordinate(double alpha) const
* Returns the coordinates of a point on the circle with angle \a alpha.
*
* \sa QPointF
*/
QPointF Circle::toCoordinate(double alpha) const
{
QPointF coordinate;
toCoordinate(coordinate, alpha);
return coordinate;
}
bool Circle::isNull() const
{
return _circleRadius <= 0 ? true : false;
}
/*!
* \class Circle
* \brief Provies a circle with radius and origin.
*
* \sa QPointF
*/
src/Wima/Geometry/Circle.h deleted 100644 → 0
View file @ a22db7f4
#pragma once
#include <QObject>
#include <QPointF>
#include <QPolygonF>
#include <cmath>
#include "PlanimetryCalculus.h"
class Circle : public QObject
{
Q_OBJECT
public:
Circle(QObject *parent = nullptr);
Circle(double radius = 0, QObject *parent = nullptr);
Circle(double radius = 0, QPointF origin = QPointF(0,0), QObject *parent = nullptr);
Circle(const Circle &other, QObject *parent = nullptr);
Circle &operator=(const Circle &other);
// Property setters
void setRadius(double radius);
void setOrigin(const QPointF &origin);
// Property getters
double radius() const;
QPointF origin() const;
// Member methodes
QVector<QPointF> approximate (int numberOfCorners) const;
QVector<QPointF> approximate (double angleDiscretisation) const;
QVector<QPointF> approximateSektor(int numberOfCorners, double alpha1, double alpha2) const;
QVector<QPointF> approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const;
void toCoordinate (QPointF &toCoordinate, double alpha) const;
QPointF toCoordinate (double alpha) const;
bool isNull() const;
signals:
void radiusChanged(double radius);
void originChanged(QPointF origin);
private:
double _circleRadius;
QPointF _circleOrigin;
};
src/Wima/Geometry/GenericCircle.h 0 → 100644
View file @ 7271b853
#pragma once
#include <QObject>
#include <QPointF>
#include <cmath>
#include "boost/units/systems/angle/degrees.hpp"
#include "boost/units/systems/si.hpp"
template <class Point, int k> auto get(Point &p);
namespace bu = boost::units;
namespace qty {
using Length = bu::quantity<bu::si::length>;
using Angle = bu::quantity<bu::si::plane_angle>;
using Degree = bu::quantity<bu::degree::plane_angle>;
using Radian = bu::quantity<bu::si::plane_angle>;
} // namespace qty
template <class NumberType, class Point> class GenericCircle {
public:
GenericCircle();
GenericCircle(NumberType rad, Point ori);
// Property setters
void setRadius(NumberType rad);
void setOrigin(const Point &ori);
// Property getters
NumberType radius() const;
const Point &origin() const;
Point &origin();
private:
NumberType _r;
Point _origin;
};
// Free functions.
template <class Circle, class Container>
void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2,
Container &c);
template <class Circle, class Container>
void approximate(Circle &circ, long n, Container &c);
// Impl.
template <class NumberType, class Point>
GenericCircle<NumberType, Point>::GenericCircle() : _r(0), _origin(0, 0) {}
template <class NumberType, class Point>
GenericCircle<NumberType, Point>::GenericCircle(NumberType rad, Point ori)
: _r(rad), _origin(ori) {}
/*!
* Returns a polygon with \a n corners which approximates the
* circle.
*
* \sa Point
*/
template <class NumberType, class Point>
void GenericCircle<NumberType, Point>::setRadius(NumberType rad) {
if (rad >= 0) {
this->_r = rad;
}
}
template <class NumberType, class Point>
void GenericCircle<NumberType, Point>::setOrigin(const Point &ori) {
this->_origin = ori;
}
template <class NumberType, class Point>
NumberType GenericCircle<NumberType, Point>::radius() const {
return this->_r;
}
template <class NumberType, class Point>
const Point &GenericCircle<NumberType, Point>::origin() const {
return this->_origin;
}
template <class NumberType, class Point>
Point &GenericCircle<NumberType, Point>::origin() {
return this->_origin;
}
template <class Circle, class Container>
void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2,
Container &c) {
auto clipp = [](double angle) {
while (angle < 0) {
angle += 2 * M_PI;
}
while (angle > 2 * M_PI) {
angle -= 2 * M_PI;
}
return angle;
};
double a1 = clipp(alpha1.value());
double a2 = clipp(alpha2.value());
double angleDisc = 0;
if (n > 0) {
angleDisc = (a2 - a1) / (n - 1);
} else {
angleDisc = (a2 - a1) / (n + 1);
n = n * (-1);
}
double a = a1;
auto x0 = get<0>(circ.origin());
auto y0 = get<1>(circ.origin());
auto r = circ.radius();
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
while (n--) {
auto x = NumberType(x0 + r * std::cos(a));
auto y = NumberType(y0 + r * std::sin(a));
c.push_back(Point(x, y));
a += angleDisc;
}
}
template <class Circle, class Container>
void approximate(Circle &circ, long n, Container &c) {
using namespace bu;
double angleDisc = 0;
if (n > 0) {
angleDisc = 2 * M_PI / (n - 1);
} else {
angleDisc = 2 * M_PI / (n + 1);
n = n * (-1);
}
double a = 0;
auto x0 = get<0>(circ.origin());
auto y0 = get<1>(circ.origin());
auto r = circ.radius();
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
while (n--) {
auto x = NumberType(x0 + r * std::cos(a));
auto y = NumberType(y0 + r * std::sin(a));
c.push_back(Point(x, y));
a += angleDisc;
}
}
src/Wima/Geometry/PlanimetryCalculus.cc
View file @ 7271b853
#include "PlanimetryCalculus.h"
#include "Circle.h"
template <int k> qreal get(QPointF &p);
template <> qreal get<0>(QPointF &p) { return p.x(); }
template <> qreal get<1>(QPointF &p) { return p.y(); }
namespace PlanimetryCalculus {
namespace {
/*!
\fn IntersectType intersects(const Circle &circle, const QLineF &line, PointList &intersectionPoints, bool calcInstersect)
Returns the Intersection type of \a circle and \a line.
Stores the intersection points in \a intersectionPoints if \a calcIntersect is \c true.
Returns \c Error if either line or circe \c {isNull() == true}.
namespace {
/*!
\fn IntersectType intersects(const Circle &circle, const QLineF &line,
PointList &intersectionPoints, bool calcInstersect) Returns the Intersection
type of \a circle and \a line. Stores the intersection points in \a
intersectionPoints if \a calcIntersect is \c true. Returns \c Error if either
line or circe \c {isNull() == true}.
\sa QPointF, Circle
*/
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints, IntersectType &type, bool calcInstersect)
{
if (!circle.isNull() && ! line.isNull()) {
*/
bool intersects(const Circle &circle, const QLineF &line,
QPointFVector &intersectionPoints, IntersectType &type,
bool calcInstersect) {
if (!line.isNull()) {
QPointF translationVector = line.p1();
double alpha = angle(line); // angle between wold and line coordinate system
QPointF originCircleL = circle.origin() - translationVector;
rotateReference(originCircleL, -alpha); // circle origin in line corrdinate system
rotateReference(originCircleL,
-alpha); // circle origin in line corrdinate system
double y = originCircleL.y();
double r = circle.radius();
if (qAbs(y) > r) {
type = NoIntersection;
return false;
}
else if ( qFuzzyCompare(qFabs(y), r) ) { // tangent
} else if (qFuzzyCompare(qFabs(y), r)) { // tangent
double x_ori = originCircleL.x();
if (x_ori >= 0 && x_ori <= line.length()) {
......@@ -46,24 +50,39 @@ namespace PlanimetryCalculus {
} else { // secant
double x_ori = originCircleL.x();
double y_ori = originCircleL.y();
double delta = qSqrt(qPow(r, 2)-qPow(y_ori, 2));
double x1 = x_ori + delta; // x coordinate (line system) of fist intersection point
double x2 = x_ori - delta;// x coordinate (line system) of second intersection point
bool doesIntersect = false; // remember if actual intersection was on the line
if (x1 >= 0 && x1 <= line.length()) { // check if intersection point is on the line
double delta = qSqrt(qPow(r, 2) - qPow(y_ori, 2));
double x1 =
x_ori +
delta; // x coordinate (line system) of fist intersection point
double x2 =
x_ori -
delta; // x coordinate (line system) of second intersection point
bool doesIntersect =
false; // remember if actual intersection was on the line
if (x1 >= 0 &&
x1 <= line.length()) { // check if intersection point is on the line
if (calcInstersect) {
QPointF intersectionPt = QPointF(x1, 0); // first intersection point (line system)
QPointF intersectionPt =
QPointF(x1, 0); // first intersection point (line system)
rotateReference(intersectionPt, alpha);
intersectionPoints.append(intersectionPt + translationVector); // transform (to world system) and append first intersection point
intersectionPoints.append(
intersectionPt +
translationVector); // transform (to world system) and append
// first intersection point
}
doesIntersect = true;
}
if (x2 >= 0 && x2 <= line.length()) { // check if intersection point is on the line
if (x2 >= 0 &&
x2 <= line.length()) { // check if intersection point is on the line
if (calcInstersect) {
QPointF intersectionPt = QPointF(x2, 0); // second intersection point (line system)
QPointF intersectionPt =
QPointF(x2, 0); // second intersection point (line system)
rotateReference(intersectionPt, alpha);
intersectionPoints.append(intersectionPt + translationVector); // transform (to world system) and append second intersection point
intersectionPoints.append(
intersectionPt +
translationVector); // transform (to world system) and append
// second intersection point
}
doesIntersect = true;
}
......@@ -74,11 +93,12 @@ namespace PlanimetryCalculus {
type = Error;
return false;
}
}
/*!
\fn bool intersects(const Circle &circle1, const Circle &circle2, PointList &intersectionPoints, IntersectType type)
Calculates the intersection points of two circles if present and stores the result in \a intersectionPoints.
/*!
\fn bool intersects(const Circle &circle1, const Circle &circle2, PointList
&intersectionPoints, IntersectType type) Calculates the intersection points
of two circles if present and stores the result in \a intersectionPoints.
Returns the intersection type of the two cirles \a circle1 and \a circle2.
The function assumes that the list \a intersectionPoints is empty.
......@@ -86,10 +106,10 @@ namespace PlanimetryCalculus {
\note Returns Error if circle.isNull() returns true;
\sa Circle
*/
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints, IntersectType type, bool calcIntersection)
{
if (!circle1.isNull() && !circle2.isNull()) {
*/
bool intersects(const Circle &circle1, const Circle &circle2,
QPointFVector &intersectionPoints, IntersectType type,
bool calcIntersection) {
double r1 = circle1.radius();
double r2 = circle2.radius();
double d = distance(circle1.origin(), circle2.origin());
......@@ -110,12 +130,10 @@ namespace PlanimetryCalculus {
type = InsideNoIntersection;
return false;
} else if (qFuzzyCompare(r + d, R)) {
if (qFuzzyIsNull(d))
{
if (qFuzzyIsNull(d)) {
type = CirclesEqual;
return true;
}
else {
} else {
type = InsideTouching;
}
} else if (d < R) {
......@@ -129,184 +147,189 @@ namespace PlanimetryCalculus {
return false;
}
if (calcIntersection) {
// calculate intersection
// Coordinate system circle1: origin = circle1.origin(), x-axis towars circle2.origin() y-axis such that the
// coordinate system is dextrorse with z-axis outward faceing with respect to the drawing plane.
double alpha = angle(circle1.origin(), circle2.origin()); // angle between world and circle1 system
QPointF translationVector = circle1.origin(); // translation vector between world and circle1 system
// Coordinate system circle1: origin = circle1.origin(), x-axis towars
// circle2.origin() y-axis such that the coordinate system is dextrorse
// with z-axis outward faceing with respect to the drawing plane.
double alpha =
angle(circle1.origin(),
circle2.origin()); // angle between world and circle1 system
QPointF translationVector =
circle1.origin(); // translation vector between world and circle1 system
if ( type == InsideTouching
|| type == OutsideTouching) {
if (type == InsideTouching || type == OutsideTouching) {
// Intersection point in coordinate system of circle 1.
// Coordinate system circle1: origin = circle1.origin(), x-axis towars circle2.origin() y-axis such that the
// coordinate system is dextrorse with z-axis outward faceing with respect to the drawing plane.
intersectionPoints.append(rotateReturn(QPointF(0, r1), alpha)+translationVector);
} else { //triggered if ( type == InsideIntersection
// Coordinate system circle1: origin = circle1.origin(), x-axis towars
// circle2.origin() y-axis such that the coordinate system is dextrorse
// with z-axis outward faceing with respect to the drawing plane.
intersectionPoints.append(rotateReturn(QPointF(0, r1), alpha) +
translationVector);
} else { // triggered if ( type == InsideIntersection
// || type == OutsideIntersection)
// See fist branch for explanation
// this equations are obtained by solving x^2+y^2=R^2 and (x - d)^2+y^2=r^2
double x = (qPow(d, 2) - qPow(r, 2) + qPow(R, 2))/2/d;
double y = 1/2/d*qSqrt(4*qPow(d*R, 2) - qPow(qPow(d, 2) - qPow(r, 2) + qPow(R, 2), 2));
// this equations are obtained by solving x^2+y^2=R^2 and (x -
// d)^2+y^2=r^2
double x = (qPow(d, 2) - qPow(r, 2) + qPow(R, 2)) / 2 / d;
double y = 1 / 2 / d *
qSqrt(4 * qPow(d * R, 2) -
qPow(qPow(d, 2) - qPow(r, 2) + qPow(R, 2), 2));
intersectionPoints.append(rotateReturn(QPointF(x, y), alpha)+translationVector);
intersectionPoints.append(rotateReturn(QPointF(x, -y), alpha)+translationVector);
intersectionPoints.append(rotateReturn(QPointF(x, y), alpha) +
translationVector);
intersectionPoints.append(rotateReturn(QPointF(x, -y), alpha) +
translationVector);
}
// Transform the coordinate to the world coordinate system. Alpha is the angle between world and circle1 coordinate system.
// Transform the coordinate to the world coordinate system. Alpha is the
// angle between world and circle1 coordinate system.
return true;
}
}
type = Error;
return false;
}
} // end anonymous namespace
}
} // end anonymous namespace
/*!
/*!
\fn void rotatePoint(QPointF &point, double alpha)
Rotates the \a point counter clockwisely by the angle \a alpha (in radiants).
*/
void rotateReference(QPointF &point, double alpha)
{
Rotates the \a point counter clockwisely by the angle \a alpha (in
radiants).
*/
void rotateReference(QPointF &point, double alpha) {
if (!point.isNull()) {
double x = point.x();
double y = point.y();
point.setX(x*qCos(alpha) - y*qSin(alpha));
point.setY(x*qSin(alpha) + y*qCos(alpha));
}
point.setX(x * qCos(alpha) - y * qSin(alpha));
point.setY(x * qSin(alpha) + y * qCos(alpha));
}
}
void rotateReference(QPointFVector &points, double alpha)
{
void rotateReference(QPointFVector &points, double alpha) {
for (int i = 0; i < points.size(); i++) {
rotateReference(points[i], alpha);
}
}
}
/*!
/*!
\fn void rotatePointDegree(QPointF &point, double alpha)
Rotates the \a point counter clockwisely by the angle \a alpha (in degrees).
*/
void rotateReferenceDegree(QPointF &point, double alpha)
{
rotateReference(point, alpha/180*M_PI);
}
*/
void rotateReferenceDegree(QPointF &point, double alpha) {
rotateReference(point, alpha / 180 * M_PI);
}
void rotateReferenceDegree(QPointFVector &points, double alpha)
{
void rotateReferenceDegree(QPointFVector &points, double alpha) {
for (int i = 0; i < points.size(); i++) {
rotateReferenceDegree(points[i], alpha);
}
}
}
/*!
/*!
\fn IntersectType intersects(const Circle &circle, const QLineF &line)
Returns the Intersection type of \a circle and \a line.
Returns \c Error if either line or circe \c {isNull() == true}.
\sa QPointF, Circle
*/
bool intersects(const Circle &circle, const QLineF &line)
{
*/
bool intersects(const Circle &circle, const QLineF &line) {
QPointFVector dummyList;
IntersectType type = NoIntersection;
return intersects(circle, line, dummyList, type, false /* calculate intersection points*/);
}
return intersects(circle, line, dummyList, type,
false /* calculate intersection points*/);
}
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints)
{
bool intersects(const Circle &circle, const QLineF &line,
QPointFVector &intersectionPoints) {
IntersectType type = NoIntersection;
return intersects(circle, line, intersectionPoints, type, true /* calculate intersection points*/);
}
return intersects(circle, line, intersectionPoints, type,
true /* calculate intersection points*/);
}
/*!
/*!
\fn double distance(const QPointF &p1, const QPointF p2)
Calculates the distance (2-norm) between \a p1 and \a p2.
\sa QPointF
*/
double distance(const QPointF &p1, const QPointF p2)
{
double dx = p2.x()-p1.x();
double dy = p2.y()-p1.y();
*/
double distance(const QPointF &p1, const QPointF p2) {
double dx = p2.x() - p1.x();
double dy = p2.y() - p1.y();
return norm(dx, dy);
}
}
/*!
/*!
\fn double distance(const QPointF &p1, const QPointF p2)
Calculates the angle (in radiants) between the line defined by \a p1 and \a p2 and the x-axis according to the following rule.
Angle = qAtan2(dy, dx), where dx = p2.x()-p1.x() and dy = p2.y()-p1.y().
Calculates the angle (in radiants) between the line defined by \a p1 and \a
p2 and the x-axis according to the following rule. Angle = qAtan2(dy, dx),
where dx = p2.x()-p1.x() and dy = p2.y()-p1.y().
\note The order of \a p1 and \a p2 matters. Swapping \a p1 and \a p2 will result in a angle of oposite sign.
\sa QPointF
*/
double angle(const QPointF &p1, const QPointF p2)
{
double dx = p2.x()-p1.x();
double dy = p2.y()-p1.y();
\note The order of \a p1 and \a p2 matters. Swapping \a p1 and \a p2 will
result in a angle of oposite sign. \sa QPointF
*/
double angle(const QPointF &p1, const QPointF p2) {
double dx = p2.x() - p1.x();
double dy = p2.y() - p1.y();
return qAtan2(dy, dx);
}
}
/*!
/*!
\fn double distance(const QPointF &p1, const QPointF p2)
Calculates the angle (in degrees) between the line defined by \a p1 and \a p2 and the x-axis according to the following rule.
Angle = qAtan2(dy, dx)*180/pi, where dx = p2.x()-p1.x() and dy = p2.y()-p1.y().
angle
\note The order of \a p1 and \a p2 matters. Swapping \a p1 and \a p2 will result in a angle of oposite sign.
\sa QPointF
*/
double angleDegree(const QPointF &p1, const QPointF p2)
{
return angle(p1, p2)*180/M_PI;
}
double truncateAngle(double angle)
{
while (angle < 0 ) { angle += 2*M_PI;}
while (angle > 2*M_PI) { angle -= 2*M_PI;}
Calculates the angle (in degrees) between the line defined by \a p1 and \a
p2 and the x-axis according to the following rule. Angle = qAtan2(dy,
dx)*180/pi, where dx = p2.x()-p1.x() and dy = p2.y()-p1.y(). angle \note The
order of \a p1 and \a p2 matters. Swapping \a p1 and \a p2 will result in a
angle of oposite sign. \sa QPointF
*/
double angleDegree(const QPointF &p1, const QPointF p2) {
return angle(p1, p2) * 180 / M_PI;
}
return angle;
double truncateAngle(double angle) {
while (angle < 0) {
angle += 2 * M_PI;
}
double truncateAngleDegree(double angle)
{
return truncateAngle(angle/180*M_PI);
while (angle > 2 * M_PI) {
angle -= 2 * M_PI;
}
return angle;
}
double truncateAngleDegree(double angle) {
return truncateAngle(angle / 180 * M_PI);
}
/*!
* \fn IntersectType intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt)
* Determines wheter \a line1 and \a line2 intersect and of which type the intersection is.
* Stores the intersection point in \a intersectionPt
* Returns the intersection type (\c IntersectType).
/*!
* \fn IntersectType intersects(const QLineF &line1, const QLineF &line2,
* QPointF &intersectionPt) Determines wheter \a line1 and \a line2 intersect
* and of which type the intersection is. Stores the intersection point in \a
* intersectionPt Returns the intersection type (\c IntersectType).
*
* Intersection Types:
* \c NoIntersection
* \c CornerCornerIntersection; A intersection is present such that two of the lines cornes touch.
* \c EdgeCornerIntersection; A intersection is present such that a corner and a edge touch.
* \c EdgeEdgeIntersection; A intersection is present such two edges intersect.
* \c LinesParallel
* \c LinesEqual
* \c Error; Returned if at least on line delivers isNULL() == true.
* \c CornerCornerIntersection; A intersection is present such that two of the
* lines cornes touch. \c EdgeCornerIntersection; A intersection is present such
* that a corner and a edge touch. \c EdgeEdgeIntersection; A intersection is
* present such two edges intersect. \c LinesParallel \c LinesEqual \c Error;
* Returned if at least on line delivers isNULL() == true.
*
*
* \sa QPointF
*/
bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt, IntersectType &type)
{
if (line1.isNull() || line2.isNull()){
bool intersects(const QLineF &line1, const QLineF &line2,
QPointF &intersectionPt, IntersectType &type) {
if (line1.isNull() || line2.isNull()) {
type = Error;
return false;
}
// line 1 coordinate system: origin line1.p1(), x-axis towards line1.p2()
QPointF translationVector = line1.p1(); // translation vector between world and line1 system
QPointF translationVector =
line1.p1(); // translation vector between world and line1 system
double alpha = angle(line1);
double l1 = line1.length();
......@@ -325,47 +348,45 @@ angle
y2 = line2L1.y1();
}
double dx = (x2-x1);
double dy = (y2-y1);
double dx = (x2 - x1);
double dy = (y2 - y1);
double xNull = 0; // (xNull, 0) intersection point in line1 system
if (!qFuzzyIsNull(dx)) {
double k = dy/dx;
double k = dy / dx;
if (qFuzzyIsNull(k)) {
if (qFuzzyIsNull(x1) && qFuzzyIsNull(y1) && qFuzzyCompare(x2, l1)){
if (qFuzzyIsNull(x1) && qFuzzyIsNull(y1) && qFuzzyCompare(x2, l1)) {
type = LinesEqual;
return true;
}
else {
} else {
type = LinesParallel;
return false;
}
}
double d = (y1*x2-y2*x1)/dx;
xNull = -d/k;
double d = (y1 * x2 - y2 * x1) / dx;
xNull = -d / k;
} else {
// lines orthogonal
if (signum(y1) != signum(y2)){
if (signum(y1) != signum(y2)) {
xNull = x1;
}
else {
} else {
type = NoIntersection;
return false;
}
}
if (xNull >= x1 && xNull <= x2){
if (xNull >= x1 && xNull <= x2) {
// determine intersection type#include "QVector3D"
if(qFuzzyIsNull(xNull) || qFuzzyCompare(xNull, l1)) {
if(qFuzzyIsNull(y1) || qFuzzyIsNull(y2))
if (qFuzzyIsNull(xNull) || qFuzzyCompare(xNull, l1)) {
if (qFuzzyIsNull(y1) || qFuzzyIsNull(y2))
type = CornerCornerIntersection;
else
type = EdgeCornerIntersection;
} else if (xNull > 0 && xNull < l1) {
if(qFuzzyIsNull(y1) || qFuzzyIsNull(y2))
if (qFuzzyIsNull(y1) || qFuzzyIsNull(y2))
type = EdgeCornerIntersection;
else
type = EdgeEdgeIntersection;
......@@ -383,40 +404,46 @@ angle
intersectionPt += translationVector;
return true;
}
}
/*!IntersectType type = NoIntersection;
* \overload bool intersects(const QPolygonF &polygon, const QLineF &line, PointList &intersectionList, NeighbourList &neighbourList, IntersectList &typeList)
* Checks if \a polygon intersect with \a line.
* Stores the intersection points in \a intersectionList.
/*!IntersectType type = NoIntersection;
* \overload bool intersects(const QPolygonF &polygon, const QLineF &line,
* PointList &intersectionList, NeighbourList &neighbourList, IntersectList
* &typeList) Checks if \a polygon intersect with \a line. Stores the
* intersection points in \a intersectionList.
*
* Stores the indices of the closest two \a area vetices for each of coorespoinding intersection points in \a neighbourList. *
* For example if an intersection point is found between the first and the second vertex of the \a area the intersection point will
* be stored in \a intersectionList and the indices 1 and 2 will be stored in \a neighbourList.
* \a neighbourList has entries of type \c {QPair<int, int>}, where \c{pair.first} would contain 1 and \c{pair.second} would contain 2, when
* relating to the above example.
* Stores the indices of the closest two \a area vetices for each of
* coorespoinding intersection points in \a neighbourList. * For example if an
* intersection point is found between the first and the second vertex of the \a
* area the intersection point will be stored in \a intersectionList and the
* indices 1 and 2 will be stored in \a neighbourList. \a neighbourList has
* entries of type \c {QPair<int, int>}, where \c{pair.first} would contain 1
* and \c{pair.second} would contain 2, when relating to the above example.
*
* Returns the \c IntersectionType of each intersection point within a QVector.
*
* \sa QPair, QVector
*/
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, NeighbourVector &neighbourList, IntersectVector &typeList)
{
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList, NeighbourVector &neighbourList,
IntersectVector &typeList) {
if (polygon.size() >= 2) {
IntersectVector intersectionTypeList;
// Assemble a line form each tow consecutive polygon vertices and check whether it intersects with line
// Assemble a line form each tow consecutive polygon vertices and check
// whether it intersects with line
for (int i = 0; i < polygon.size(); i++) {
QLineF interatorLine;
QPointF currentVertex = polygon[i];
QPointF nextVertex = polygon[PolygonCalculus::nextVertexIndex(polygon.size(), i)];
QPointF nextVertex =
polygon[PolygonCalculus::nextVertexIndex(polygon.size(), i)];
interatorLine.setP1(currentVertex);
interatorLine.setP2(nextVertex);
QPointF intersectionPoint;
IntersectType type;
if ( intersects(line, interatorLine, intersectionPoint, type) ) {
if (intersects(line, interatorLine, intersectionPoint, type)) {
intersectionList.append(intersectionPoint);
QPair<int, int> neighbours;
......@@ -436,16 +463,16 @@ angle
} else {
return false;
}
}
}
/*!
* \overload IntersectType intersects(const QPolygonF &polygon, const QLineF &line)
* Returns \c true if any intersection between \a polygon and \a line exists, \c false else.
/*!
* \overload IntersectType intersects(const QPolygonF &polygon, const QLineF
* &line) Returns \c true if any intersection between \a polygon and \a line
* exists, \c false else.
*
* \sa QPair, QVector
*/
bool intersects(const QPolygonF &polygon, const QLineF &line)
{
bool intersects(const QPolygonF &polygon, const QLineF &line) {
QPointFVector dummyGeo;
QVector<QPair<int, int>> dummyNeighbour;
intersects(polygon, line, dummyGeo, dummyNeighbour);
......@@ -454,100 +481,93 @@ angle
return true;
return false;
}
}
void rotateReference(QLineF &line, double alpha)
{
void rotateReference(QLineF &line, double alpha) {
line.setP1(rotateReturn(line.p1(), alpha));
line.setP2(rotateReturn(line.p2(), alpha));
}
}
QPointF rotateReturn(QPointF point, double alpha)
{
QPointF rotateReturn(QPointF point, double alpha) {
rotateReference(point, alpha);
return point;
}
}
QPointFVector rotateReturn(QPointFVector points, double alpha)
{
QPointFVector rotateReturn(QPointFVector points, double alpha) {
rotateReference(points, alpha);
return points;
}
}
QLineF rotateReturn(QLineF line, double alpha)
{
QLineF rotateReturn(QLineF line, double alpha) {
rotateReference(line, alpha);
return line;
}
}
bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt)
{
bool intersects(const QLineF &line1, const QLineF &line2,
QPointF &intersectionPt) {
IntersectType dummyType;
return intersects(line1, line2, intersectionPt, dummyType);
}
}
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, NeighbourVector &neighbourList)
{
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList,
NeighbourVector &neighbourList) {
IntersectVector typeList;
return intersects(polygon, line, intersectionList, neighbourList, typeList);
}
}
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, IntersectVector &typeList)
{
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList, IntersectVector &typeList) {
NeighbourVector neighbourList;
return intersects(polygon, line, intersectionList, neighbourList, typeList);
}
}
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList)
{
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList) {
NeighbourVector neighbourList;
IntersectVector typeList;
return intersects(polygon, line, intersectionList, neighbourList, typeList);
}
}
/*!
/*!
\fn IntersectType intersects(const Circle &circle1, const Circle &circle2)
Returns the intersection type of the two cirles \a circle1 and \a circle2.
\note Returns Error if circle.isNull() returns true;
\sa Circle
*/
bool intersects(const Circle &circle1, const Circle &circle2)
{
*/
bool intersects(const Circle &circle1, const Circle &circle2) {
IntersectType type = NoIntersection;
QPointFVector intersectionPoints;
return intersects(circle1, circle2, intersectionPoints, type, false /*calculate intersection points*/);
}
return intersects(circle1, circle2, intersectionPoints, type,
false /*calculate intersection points*/);
}
bool intersects(const Circle &circle1, const Circle &circle2, IntersectType &type)
{
bool intersects(const Circle &circle1, const Circle &circle2,
IntersectType &type) {
QPointFVector intersectionPoints;
return intersects(circle1, circle2, intersectionPoints, type, false /*calculate intersection points*/);
}
return intersects(circle1, circle2, intersectionPoints, type,
false /*calculate intersection points*/);
}
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints)
{
bool intersects(const Circle &circle1, const Circle &circle2,
QPointFVector &intersectionPoints) {
IntersectType type;
return intersects(circle1, circle2, intersectionPoints, type);
}
}
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints, IntersectType &type)
{
return intersects(circle1, circle2, intersectionPoints, type, true /*calculate intersection points*/);
}
bool intersects(const Circle &circle1, const Circle &circle2,
QPointFVector &intersectionPoints, IntersectType &type) {
return intersects(circle1, circle2, intersectionPoints, type,
true /*calculate intersection points*/);
}
;
;
double angle(const QLineF &line)
{
return angle(line.p1(), line.p2());
}
double angle(const QLineF &line) { return angle(line.p1(), line.p2()); }
bool contains(const QLineF &line, const QPointF &point, IntersectType &type)
{
bool contains(const QLineF &line, const QPointF &point, IntersectType &type) {
QPointF translationVector = line.p1();
double alpha = angle(line);
double l = line.length();
......@@ -557,16 +577,14 @@ angle
double x = pointL1.x();
double y = pointL1.y();
if ( x >= 0 && x <= l ) {
if (x >= 0 && x <= l) {
if (qFuzzyIsNull(x) || qFuzzyCompare(x, l)) {
if (qFuzzyIsNull(y))
{
if (qFuzzyIsNull(y)) {
type = CornerCornerIntersection;
return true;
}
} else {
if (qFuzzyIsNull(y))
{
if (qFuzzyIsNull(y)) {
type = EdgeCornerIntersection;
return true;
}
......@@ -574,57 +592,47 @@ angle
}
type = NoIntersection;
return false;
}
}
bool contains(const QLineF &line, const QPointF &point)
{
bool contains(const QLineF &line, const QPointF &point) {
IntersectType dummyType;
return contains(line, point, dummyType);
}
}
bool contains(const QPolygonF &polygon, const QPointF &point, IntersectType &type)
{
bool contains(const QPolygonF &polygon, const QPointF &point,
IntersectType &type) {
using namespace PolygonCalculus;
if (polygon.containsPoint(point, Qt::FillRule::OddEvenFill))
{
if (polygon.containsPoint(point, Qt::FillRule::OddEvenFill)) {
type = Interior;
return true;
}
int size = polygon.size();
for (int i = 0; i < size; i++) {
QLineF line(polygon[i], polygon[nextVertexIndex(size, i)]);
if ( contains(line, point, type) ) {
if (contains(line, point, type)) {
return true;
}
}
return false;
}
}
bool contains(const QPolygonF &polygon, const QPointF &point)
{
bool contains(const QPolygonF &polygon, const QPointF &point) {
IntersectType dummyType;
return contains(polygon, point, dummyType);
}
}
double norm(double x, double y)
{
return qSqrt(x*x+y*y);
}
double norm(double x, double y) { return qSqrt(x * x + y * y); }
double norm(const QPointF &p)
{
return norm(p.x(), p.y());
}
double norm(const QPointF &p) { return norm(p.x(), p.y()); }
double angle(const QPointF &p1)
{
double angle(const QPointF &p1) {
return truncateAngle(qAtan2(p1.y(), p1.x()));
}
}
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, NeighbourVector &neighbourList, IntersectVector &typeList)
{
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
NeighbourVector &neighbourList, IntersectVector &typeList) {
using namespace PolygonCalculus;
for (int i = 0; i < polygon.size(); i++) {
QPointF p1 = polygon[i];
......@@ -642,7 +650,6 @@ angle
typeList.append(type);
intersectionPoints.append(lineIntersecPts);
}
}
if (intersectionPoints.size() > 0)
......@@ -650,57 +657,58 @@ angle
else {
return false;
}
}
}
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, NeighbourVector &neighbourList)
{
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
NeighbourVector &neighbourList) {
QVector<IntersectType> types;
return intersects(circle, polygon, intersectionPoints, neighbourList, types);
}
}
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, IntersectVector &typeList)
{
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
IntersectVector &typeList) {
NeighbourVector neighbourList;
return intersects(circle, polygon, intersectionPoints, neighbourList, typeList);
}
return intersects(circle, polygon, intersectionPoints, neighbourList,
typeList);
}
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints)
{
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints) {
NeighbourVector neighbourList;
return intersects(circle, polygon, intersectionPoints, neighbourList);
}
}
bool intersects(const Circle &circle, const QPolygonF &polygon)
{
bool intersects(const Circle &circle, const QPolygonF &polygon) {
QVector<QPointFVector> intersectionPoints;
return intersects(circle, polygon, intersectionPoints);
}
}
bool intersects(const QLineF &line1, const QLineF &line2)
{
bool intersects(const QLineF &line1, const QLineF &line2) {
QPointF intersectionPoint;
return intersects(line1, line2, intersectionPoint);
}
}
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints, IntersectType &type)
{
return intersects(circle, line, intersectionPoints, type, true /* calculate intersection points*/);
}
bool intersects(const Circle &circle, const QLineF &line,
QPointFVector &intersectionPoints, IntersectType &type) {
return intersects(circle, line, intersectionPoints, type,
true /* calculate intersection points*/);
}
bool intersectsFast(const QPolygonF &polygon, const QLineF &line)
{
for (int i = 0; i < polygon.size()-1; ++i) {
QLineF polygonSegment(polygon[i], polygon[i+1]);
bool intersectsFast(const QPolygonF &polygon, const QLineF &line) {
for (int i = 0; i < polygon.size() - 1; ++i) {
QLineF polygonSegment(polygon[i], polygon[i + 1]);
if(QLineF::BoundedIntersection == line.intersect(polygonSegment, nullptr))
if (QLineF::BoundedIntersection == line.intersect(polygonSegment, nullptr))
return true;
}
return false;
}
}
// bool intersectsFast(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt)
// bool intersectsFast(const QLineF &line1, const QLineF &line2, QPointF
// &intersectionPt)
// {
// if (line1.isNull() || line2.isNull()){
// return false;
......@@ -718,7 +726,6 @@ angle
// d1 = line1.y1()-k1*line1.x1();
// }
// double dx2 = line2.dx();
// double dy2 = line2.dy();
// double k2;
......@@ -731,14 +738,16 @@ angle
// }
// if ( !qFuzzyCompare(k1, std::numeric_limits<double>::infinity())
// && !qFuzzyCompare(k1, std::numeric_limits<double>::infinity())) {
// && !qFuzzyCompare(k1, std::numeric_limits<double>::infinity()))
// {
// double dk = k2-k1;
// double dd = d2-d1;
// if (qFuzzyIsNull(dk)) { // lines parallel
// if (qFuzzyIsNull(dd)) { // lines colinear
// if ( ((line1.x1() >= line2.x1()) && ((line1.x1() <= line2.x2())))
// || ((line1.x2() >= line2.x1()) && ((line1.x2() <= line2.x2()))) ) // intersect?
// return true;
// if ( ((line1.x1() >= line2.x1()) && ((line1.x1() <=
// line2.x2())))
// || ((line1.x2() >= line2.x1()) && ((line1.x2() <=
// line2.x2()))) ) // intersect? return true;
// return false;
// }
......@@ -749,31 +758,17 @@ angle
// if (((x_intersect >= line2.x1()) && ((line1.x1() <= line2.x2())))
// }
// return true;
// }
} // end namespace PlanimetryCalculus
/*!
\class PlanimetryCalculus
\inmodule Wima
\brief The \c PlanimetryCalculus class provides routines handy for planimetrical (2D) calculations.
\brief The \c PlanimetryCalculus class provides routines handy for
planimetrical (2D) calculations.
*/
src/Wima/Geometry/PlanimetryCalculus.h
View file @ 7271b853
......@@ -4,97 +4,121 @@
#include <QPointF>
#include <QPolygonF>
#include <QtMath>
#include <QLineF>
#include "GenericCircle.h"
#include "PolygonCalculus.h"
class Circle;
using Circle = GenericCircle<qreal, QPointF>;
namespace PlanimetryCalculus {
enum IntersectType{InsideNoIntersection, InsideTouching, InsideIntersection,
OutsideIntersection, OutsideTouching, OutsideNoIntersection,
CirclesEqual, //Circle Circle intersection
enum IntersectType {
InsideNoIntersection,
InsideTouching,
InsideIntersection,
OutsideIntersection,
OutsideTouching,
OutsideNoIntersection,
CirclesEqual, // Circle Circle intersection
Tangent, Secant, // Circle Line Intersetion
Tangent,
Secant, // Circle Line Intersetion
EdgeCornerIntersection, EdgeEdgeIntersection, CornerCornerIntersection,
LinesParallel, LinesEqual, // Line Line intersection
EdgeCornerIntersection,
EdgeEdgeIntersection,
CornerCornerIntersection,
LinesParallel,
LinesEqual, // Line Line intersection
Interior, // Polygon contains
NoIntersection, Error // general
};
typedef QVector<QPair<int, int>> NeighbourVector;
typedef QVector<QPointF> QPointFVector;
typedef QVector<IntersectType> IntersectVector;
void rotateReference(QPointF &point, double alpha);
void rotateReference(QPointFVector &points, double alpha);
void rotateReference(QLineF &line, double alpha);
//void rotateReference(QPolygonF &polygon, double alpha);
QPointF rotateReturn(QPointF point, double alpha);
QPointFVector rotateReturn(QPointFVector points, double alpha);
QLineF rotateReturn(QLineF line, double alpha);
//QPolygonF rotateReturn(QPolygonF &polygon, double alpha);
bool intersects(const Circle &circle1, const Circle &circle2);
bool intersects(const Circle &circle1, const Circle &circle2, IntersectType &type);
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints);
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints, IntersectType &type);
bool intersects(const Circle &circle, const QLineF &line);
bool intersects(const Circle &circle, const QLineF &line, IntersectType &type);
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints);
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints, IntersectType &type);
bool intersects(const Circle &circle, const QPolygonF &polygon);
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints);
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, IntersectVector &typeList);
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, NeighbourVector &neighbourList);
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, NeighbourVector &neighbourList, IntersectVector &typeList);
bool intersects(const QLineF &line1, const QLineF &line2);
bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt);
bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt, IntersectType &type);
// bool intersectsFast(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt, IntersectType &type);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, IntersectVector &typeList);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, NeighbourVector &neighbourList);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, NeighbourVector &neighbourList, IntersectVector &typeList);
bool intersectsFast(const QPolygonF &polygon, const QLineF &line);
bool contains(const QLineF &line, const QPointF &point);
bool contains(const QLineF &line, const QPointF &point, IntersectType &type);
bool contains(const QPolygonF &polygon, const QPointF &point);
bool contains(const QPolygonF &polygon, const QPointF &point, IntersectType &type);
double distance(const QPointF &p1, const QPointF p2);
double norm(double x, double y);
double norm(const QPointF &p);
double angle(const QPointF &p1, const QPointF p2);
double angle(const QPointF &p1);
double angle(const QLineF &line);
double angleDegree(const QPointF &p1, const QPointF p2);
double truncateAngle(double angle);
double truncateAngleDegree(double angle);
/*!
NoIntersection,
Error // general
};
typedef QVector<QPair<int, int>> NeighbourVector;
typedef QVector<QPointF> QPointFVector;
typedef QVector<IntersectType> IntersectVector;
void rotateReference(QPointF &point, double alpha);
void rotateReference(QPointFVector &points, double alpha);
void rotateReference(QLineF &line, double alpha);
// void rotateReference(QPolygonF &polygon, double alpha);
QPointF rotateReturn(QPointF point, double alpha);
QPointFVector rotateReturn(QPointFVector points, double alpha);
QLineF rotateReturn(QLineF line, double alpha);
// QPolygonF rotateReturn(QPolygonF &polygon, double alpha);
bool intersects(const Circle &circle1, const Circle &circle2);
bool intersects(const Circle &circle1, const Circle &circle2,
IntersectType &type);
bool intersects(const Circle &circle1, const Circle &circle2,
QPointFVector &intersectionPoints);
bool intersects(const Circle &circle1, const Circle &circle2,
QPointFVector &intersectionPoints, IntersectType &type);
bool intersects(const Circle &circle, const QLineF &line);
bool intersects(const Circle &circle, const QLineF &line, IntersectType &type);
bool intersects(const Circle &circle, const QLineF &line,
QPointFVector &intersectionPoints);
bool intersects(const Circle &circle, const QLineF &line,
QPointFVector &intersectionPoints, IntersectType &type);
bool intersects(const Circle &circle, const QPolygonF &polygon);
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints);
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
IntersectVector &typeList);
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
NeighbourVector &neighbourList);
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
NeighbourVector &neighbourList, IntersectVector &typeList);
bool intersects(const QLineF &line1, const QLineF &line2);
bool intersects(const QLineF &line1, const QLineF &line2,
QPointF &intersectionPt);
bool intersects(const QLineF &line1, const QLineF &line2,
QPointF &intersectionPt, IntersectType &type);
// bool intersectsFast(const QLineF &line1, const QLineF &line2, QPointF
// &intersectionPt, IntersectType &type);
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList);
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList, IntersectVector &typeList);
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList,
NeighbourVector &neighbourList);
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList, NeighbourVector &neighbourList,
IntersectVector &typeList);
bool intersectsFast(const QPolygonF &polygon, const QLineF &line);
bool contains(const QLineF &line, const QPointF &point);
bool contains(const QLineF &line, const QPointF &point, IntersectType &type);
bool contains(const QPolygonF &polygon, const QPointF &point);
bool contains(const QPolygonF &polygon, const QPointF &point,
IntersectType &type);
double distance(const QPointF &p1, const QPointF p2);
double norm(double x, double y);
double norm(const QPointF &p);
double angle(const QPointF &p1, const QPointF p2);
double angle(const QPointF &p1);
double angle(const QLineF &line);
double angleDegree(const QPointF &p1, const QPointF p2);
double truncateAngle(double angle);
double truncateAngleDegree(double angle);
/*!
* \fntemplate <typename T> int signum(T val)
* Returns the signum of a value \a val.
*
* \sa QPair, QVector
*/
template <typename T> int signum(T val)
{
return (T(0) < val) - (val < T(0));
}
}
template <typename T> int signum(T val) { return (T(0) < val) - (val < T(0)); }
} // namespace PlanimetryCalculus
src/Wima/Snake/SnakeThread.cc
View file @ 7271b853
......@@ -397,6 +397,8 @@ void SnakeThread::run() {
waypointsValid = false;
}
}
} else {
waypointsValid = false;
}
UniqueLock lk(this->pImpl->output.mutex);
......
src/Wima/Snake/snake.h
View file @ 7271b853
......@@ -167,7 +167,7 @@ bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth,
using Transects = vector<BoostLineString>;
using Progress = vector<int>;
using Route = vector<BoostPoint>;
using Route = BoostLineString;
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const BoostPolygon &mArea,
......
src/Wima/WimaPlaner.cc
View file @ 7271b853
#include "WimaPlaner.h"
#include "WimaPlaner.h"
#include "CircularSurveyComplexItem.h"
#include "MissionController.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "QGCMapPolygon.h"
#include "SimpleMissionItem.h"
#include "Geometry/GeoUtilities.h"
#include "Geometry/PlanimetryCalculus.h"
#include "OptimisationTools.h"
#include "CircularSurvey.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaAreaData.h"
#include "WimaBridge.h"
const char* WimaPlaner::wimaFileExtension = "wima";
const char* WimaPlaner::areaItemsName = "AreaItems";
const char* WimaPlaner::missionItemsName = "MissionItems";
const char *WimaPlaner::wimaFileExtension = "wima";
const char *WimaPlaner::areaItemsName = "AreaItems";
const char *WimaPlaner::missionItemsName = "MissionItems";
WimaPlaner::WimaPlaner(QObject *parent)
: QObject (parent)
, _currentAreaIndex (-1)
, _wimaBridge (nullptr)
, _joinedArea (this)
, _joinedAreaValid (false)
, _measurementArea (this)
, _serviceArea (this)
, _corridor (this)
, _circularSurvey (nullptr)
, _surveyRefChanging (false)
, _measurementAreaChanging (false)
, _corridorChanging (false)
, _serviceAreaChanging (false)
, _syncronizedWithController (false)
, _readyForSync (false)
{
: QObject(parent), _currentAreaIndex(-1), _wimaBridge(nullptr),
_joinedArea(this), _joinedAreaValid(false), _measurementArea(this),
_serviceArea(this), _corridor(this), _TSComplexItem(nullptr),
_surveyRefChanging(false), _measurementAreaChanging(false),
_corridorChanging(false), _serviceAreaChanging(false),
_syncronizedWithController(false), _readyForSync(false) {
_lastMeasurementAreaPath = _measurementArea.path();
_lastServiceAreaPath = _serviceArea.path();
_lastCorridorPath = _corridor.path();
connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::recalcPolygonInteractivity);
connect(this, &WimaPlaner::currentPolygonIndexChanged, this,
&WimaPlaner::recalcPolygonInteractivity);
connect(&_updateTimer, &QTimer::timeout, this, &WimaPlaner::updateTimerSlot);
connect(this, &WimaPlaner::joinedAreaValidChanged, this, &WimaPlaner::updateMission);
connect(this, &WimaPlaner::joinedAreaValidChanged, this,
&WimaPlaner::updateMission);
_updateTimer.setInterval(300); // 300 ms means: max update time 2*300 ms
_updateTimer.start();
#ifndef NDEBUG
// for debugging and testing purpose, remove if not needed anymore
connect(&_autoLoadTimer, &QTimer::timeout, this, &WimaPlaner::autoLoadMission);
connect(&_autoLoadTimer, &QTimer::timeout, this,
&WimaPlaner::autoLoadMission);
_autoLoadTimer.setSingleShot(true);
_autoLoadTimer.start(300);
#endif
_calcArrivalAndReturnPathTimer.setInterval(100);
_calcArrivalAndReturnPathTimer.setSingleShot(true);
connect(&_calcArrivalAndReturnPathTimer, &QTimer::timeout, this, &WimaPlaner::calcArrivalAndReturnPath);
connect(&_calcArrivalAndReturnPathTimer, &QTimer::timeout, this,
&WimaPlaner::calcArrivalAndReturnPath);
}
QmlObjectListModel* WimaPlaner::visualItems()
{
return &_visualItems;
}
QmlObjectListModel *WimaPlaner::visualItems() { return &_visualItems; }
QStringList WimaPlaner::loadNameFilters() const
{
QStringList WimaPlaner::loadNameFilters() const {
QStringList filters;
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
tr("All Files (*.*)");
filters << tr("Supported types (*.%1 *.%2)")
.arg(wimaFileExtension)
.arg(AppSettings::planFileExtension)
<< tr("All Files (*.*)");
return filters;
}
QStringList WimaPlaner::saveNameFilters() const
{
QStringList WimaPlaner::saveNameFilters() const {
QStringList filters;
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
filters << tr("Supported types (*.%1 *.%2)")
.arg(wimaFileExtension)
.arg(AppSettings::planFileExtension);
return filters;
}
QGeoCoordinate WimaPlaner::joinedAreaCenter() const
{
QGeoCoordinate WimaPlaner::joinedAreaCenter() const {
return _joinedArea.center();
}
void WimaPlaner::setMasterController(PlanMasterController *masterC)
{
void WimaPlaner::setMasterController(PlanMasterController *masterC) {
_masterController = masterC;
emit masterControllerChanged();
}
void WimaPlaner::setMissionController(MissionController *missionC)
{
void WimaPlaner::setMissionController(MissionController *missionC) {
_missionController = missionC;
emit missionControllerChanged();
}
void WimaPlaner::setCurrentPolygonIndex(int index)
{
if(index >= 0 && index < _visualItems.count() && index != _currentAreaIndex){
void WimaPlaner::setCurrentPolygonIndex(int index) {
if (index >= 0 && index < _visualItems.count() &&
index != _currentAreaIndex) {
_currentAreaIndex = index;
emit currentPolygonIndexChanged(index);
}
}
void WimaPlaner::setWimaBridge(WimaBridge *bridge)
{
void WimaPlaner::setWimaBridge(WimaBridge *bridge) {
if (bridge != nullptr) {
_wimaBridge = bridge;
emit wimaBridgeChanged();
}
}
bool WimaPlaner::syncronizedWithController()
{
bool WimaPlaner::syncronizedWithController() {
return _syncronizedWithController;
}
bool WimaPlaner::readyForSync()
{
return _readyForSync;
}
bool WimaPlaner::readyForSync() { return _readyForSync; }
WimaPlaner *WimaPlaner::thisPointer()
{
return this;
}
WimaPlaner *WimaPlaner::thisPointer() { return this; }
void WimaPlaner::removeArea(int index)
{
if(index >= 0 && index < _visualItems.count()){
WimaArea* area = qobject_cast<WimaArea*>(_visualItems.removeAt(index));
void WimaPlaner::removeArea(int index) {
if (index >= 0 && index < _visualItems.count()) {
WimaArea *area = qobject_cast<WimaArea *>(_visualItems.removeAt(index));
if ( area == nullptr) {
if (area == nullptr) {
qWarning("WimaPlaner::removeArea(): nullptr catched, internal error.");
return;
}
......@@ -136,29 +128,28 @@ void WimaPlaner::removeArea(int index)
if (_visualItems.count() == 0) {
// this branch is reached if all items are removed
// to guarentee proper behavior, _currentAreaIndex must be set to a invalid value, as on constructor init.
// to guarentee proper behavior, _currentAreaIndex must be set to a
// invalid value, as on constructor init.
resetAllInteractive();
_currentAreaIndex = -1;
return;
}
if(_currentAreaIndex >= _visualItems.count()){
if (_currentAreaIndex >= _visualItems.count()) {
setCurrentPolygonIndex(_visualItems.count() - 1);
}else{
} else {
recalcPolygonInteractivity(_currentAreaIndex);
}
}else{
} else {
qWarning("Index out of bounds!");
}
}
bool WimaPlaner::addMeasurementArea()
{
bool WimaPlaner::addMeasurementArea() {
if (!_visualItems.contains(&_measurementArea)) {
_visualItems.append(&_measurementArea);
int newIndex = _visualItems.count()-1;
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
......@@ -168,12 +159,11 @@ bool WimaPlaner::addMeasurementArea()
}
}
bool WimaPlaner::addServiceArea()
{
bool WimaPlaner::addServiceArea() {
if (!_visualItems.contains(&_serviceArea)) {
_visualItems.append(&_serviceArea);
int newIndex = _visualItems.count()-1;
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
......@@ -183,12 +173,11 @@ bool WimaPlaner::addServiceArea()
}
}
bool WimaPlaner::addCorridor()
{
bool WimaPlaner::addCorridor() {
if (!_visualItems.contains(&_corridor)) {
_visualItems.append(&_corridor);
int newIndex = _visualItems.count()-1;
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
......@@ -198,8 +187,7 @@ bool WimaPlaner::addCorridor()
}
}
void WimaPlaner::removeAll()
{
void WimaPlaner::removeAll() {
bool changesApplied = false;
while (_visualItems.count() > 0) {
removeArea(0);
......@@ -210,64 +198,61 @@ void WimaPlaner::removeAll()
_currentFile = "";
_circularSurvey = nullptr;
_TSComplexItem = nullptr;
_surveyRefChanging = false;
emit currentFileChanged();
if ( changesApplied )
if (changesApplied)
emit visualItemsChanged();
}
bool WimaPlaner::updateMission()
{
bool WimaPlaner::updateMission() {
setReadyForSync(false);
if ( !_joinedAreaValid)
if (!_joinedAreaValid)
return false;
// extract old survey data
QmlObjectListModel* missionItems = _missionController->visualItems();
QmlObjectListModel *missionItems = _missionController->visualItems();
int surveyIndex = missionItems->indexOf(_circularSurvey);
int surveyIndex = missionItems->indexOf(_TSComplexItem);
// create survey item if not yet present
if (surveyIndex == -1) {
_missionController->insertComplexMissionItem(_missionController->circularSurveyComplexItemName(), _measurementArea.center(), missionItems->count());
_circularSurvey = qobject_cast<CircularSurveyComplexItem*>(missionItems->get(missionItems->count()-1));
_missionController->insertComplexMissionItem(
_missionController->circularSurveyComplexItemName(),
_measurementArea.center(), missionItems->count());
_TSComplexItem = qobject_cast<CircularSurvey *>(
missionItems->get(missionItems->count() - 1));
if (_circularSurvey == nullptr){
if (_TSComplexItem == nullptr) {
qWarning("WimaPlaner::updateMission(): survey == nullptr");
return false;
}
// establish connections
_circularSurvey->setRefPoint(_measurementArea.center());
_TSComplexItem->setRefPoint(_measurementArea.center());
_lastSurveyRefPoint = _measurementArea.center();
_surveyRefChanging = false;
_circularSurvey->setIsInitialized(true); // prevents reinitialisation from gui
connect(_circularSurvey, &TransectStyleComplexItem::missionItemReady, this, &WimaPlaner::calcArrivalAndReturnPath);
_TSComplexItem->setIsInitialized(
true); // prevents reinitialisation from gui
connect(_TSComplexItem, &TransectStyleComplexItem::missionItemReady, this,
&WimaPlaner::calcArrivalAndReturnPath);
}
// update survey area
_circularSurvey->surveyAreaPolygon()->clear();
_circularSurvey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList());
_circularSurvey->comprehensiveUpdate();
_TSComplexItem->surveyAreaPolygon()->clear();
_TSComplexItem->surveyAreaPolygon()->appendVertices(
_measurementArea.coordinateList());
setReadyForSync(true);
return true;
}
void WimaPlaner::saveToCurrent()
{
saveToFile(_currentFile);
}
void WimaPlaner::saveToCurrent() { saveToFile(_currentFile); }
void WimaPlaner::saveToFile(const QString& filename)
{
void WimaPlaner::saveToFile(const QString &filename) {
if (filename.isEmpty()) {
return;
}
......@@ -279,17 +264,19 @@ void WimaPlaner::saveToFile(const QString& filename)
QFile file(planFilename);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
qgcApp()->showMessage(tr("Plan save error %1 : %2").arg(filename).arg(file.errorString()));
qgcApp()->showMessage(
tr("Plan save error %1 : %2").arg(filename).arg(file.errorString()));
_currentFile.clear();
emit currentFileChanged();
} else {
FileType fileType = FileType::WimaFile;
if ( planFilename.contains(QString(".%1").arg(wimaFileExtension)) ) {
if (planFilename.contains(QString(".%1").arg(wimaFileExtension))) {
fileType = FileType::WimaFile;
} else if ( planFilename.contains(QString(".%1").arg(AppSettings::planFileExtension)) ) {
} else if (planFilename.contains(
QString(".%1").arg(AppSettings::planFileExtension))) {
fileType = FileType::PlanFile;
} else {
if ( planFilename.contains(".") ) {
if (planFilename.contains(".")) {
qgcApp()->showMessage(tr("File format not supported"));
} else {
qgcApp()->showMessage(tr("File without file extension not accepted."));
......@@ -299,23 +286,20 @@ void WimaPlaner::saveToFile(const QString& filename)
QJsonDocument saveDoc = saveToJson(fileType);
file.write(saveDoc.toJson());
if(_currentFile != planFilename) {
if (_currentFile != planFilename) {
_currentFile = planFilename;
emit currentFileChanged();
}
}
}
bool WimaPlaner::loadFromCurrent()
{
return loadFromFile(_currentFile);
}
bool WimaPlaner::loadFromCurrent() { return loadFromFile(_currentFile); }
bool WimaPlaner::loadFromFile(const QString &filename)
{
#define debug 0
bool WimaPlaner::loadFromFile(const QString &filename) {
#define debug 0
QString errorString;
QString errorMessage = tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");
QString errorMessage =
tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");
if (filename.isEmpty()) {
return false;
......@@ -330,7 +314,7 @@ bool WimaPlaner::loadFromFile(const QString &filename)
return false;
}
if(fileInfo.suffix() == wimaFileExtension) {
if (fileInfo.suffix() == wimaFileExtension) {
QJsonDocument jsonDoc;
QByteArray bytes = file.readAll();
......@@ -345,86 +329,100 @@ bool WimaPlaner::loadFromFile(const QString &filename)
_visualItems.clear();
int validAreaCounter = 0;
for( int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) {
for (int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) {
QJsonObject jsonArea = areaArray[i].toObject();
if (jsonArea.contains(WimaArea::areaTypeName) && jsonArea[WimaArea::areaTypeName].isString()) {
if ( jsonArea[WimaArea::areaTypeName] == WimaMeasurementArea::WimaMeasurementAreaName) {
if (jsonArea.contains(WimaArea::areaTypeName) &&
jsonArea[WimaArea::areaTypeName].isString()) {
if (jsonArea[WimaArea::areaTypeName] ==
WimaMeasurementArea::WimaMeasurementAreaName) {
bool success = _measurementArea.loadFromJson(jsonArea, errorString);
if ( !success ) {
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
_lastMeasurementAreaPath = _measurementArea.path(); // prevents error messages at this point
_lastMeasurementAreaPath =
_measurementArea.path(); // prevents error messages at this point
validAreaCounter++;
_visualItems.append(&_measurementArea);
emit visualItemsChanged();
} else if ( jsonArea[WimaArea::areaTypeName] == WimaServiceArea::wimaServiceAreaName) {
} else if (jsonArea[WimaArea::areaTypeName] ==
WimaServiceArea::wimaServiceAreaName) {
bool success = _serviceArea.loadFromJson(jsonArea, errorString);
if ( !success ) {
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
_lastServiceAreaPath = _serviceArea.path(); // prevents error messages at this point
_lastServiceAreaPath =
_serviceArea.path(); // prevents error messages at this point
validAreaCounter++;
_visualItems.append(&_serviceArea);
emit visualItemsChanged();
} else if ( jsonArea[WimaArea::areaTypeName] == WimaCorridor::WimaCorridorName) {
} else if (jsonArea[WimaArea::areaTypeName] ==
WimaCorridor::WimaCorridorName) {
bool success = _corridor.loadFromJson(jsonArea, errorString);
if ( !success ) {
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
_lastCorridorPath = _corridor.path(); // prevents error messages at this point
_lastCorridorPath =
_corridor.path(); // prevents error messages at this point
validAreaCounter++;
_visualItems.append(&_corridor);
emit visualItemsChanged();
} else {
errorString += QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName));
errorString +=
QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName));
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
} else {
errorString += QString(tr("Invalid or non existing entry for %s.\n").arg(WimaArea::areaTypeName));
errorString += QString(tr("Invalid or non existing entry for %s.\n")
.arg(WimaArea::areaTypeName));
return false;
}
}
_currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(), fileInfo.completeBaseName().toLocal8Bit().data(), wimaFileExtension);
_currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(),
fileInfo.completeBaseName().toLocal8Bit().data(),
wimaFileExtension);
emit currentFileChanged();
QJsonObject missionObject = json[missionItemsName].toObject();
//qWarning() << json[missionItemsName].type();
// qWarning() << json[missionItemsName].type();
QJsonDocument missionJsonDoc = QJsonDocument(missionObject);
// create temporary file with missionItems
QFile temporaryFile;
QString cropedFileName = filename.section("/",0,-2);
#if debug
QString cropedFileName = filename.section("/", 0, -2);
#if debug
qWarning() << cropedFileName;
#endif
#endif
QString temporaryFileName;
for (int i = 0; ; i++) {
temporaryFileName = cropedFileName + QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);
for (int i = 0;; i++) {
temporaryFileName =
cropedFileName +
QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);
// qWarning() << temporaryFileName;
if ( !QFile::exists(temporaryFileName) ) {
if (!QFile::exists(temporaryFileName)) {
temporaryFile.setFileName(temporaryFileName);
if ( temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text) ) {
if (temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text)) {
break;
}
}
if ( i > 1000) {
qWarning("WimaPlaner::loadFromFile(): not able to create temporary file.");
if (i > 1000) {
qWarning(
"WimaPlaner::loadFromFile(): not able to create temporary file.");
return false;
}
}
......@@ -438,38 +436,41 @@ bool WimaPlaner::loadFromFile(const QString &filename)
QmlObjectListModel *missionItems = _missionController->visualItems();
_circularSurvey = nullptr;
_TSComplexItem = nullptr;
for (int i = 0; i < missionItems->count(); i++) {
_circularSurvey = missionItems->value<CircularSurveyComplexItem *>(i);
if (_circularSurvey != nullptr) {
_TSComplexItem = missionItems->value<CircularSurvey *>(i);
if (_TSComplexItem != nullptr) {
_lastSurveyRefPoint = _circularSurvey->refPoint();
_lastSurveyRefPoint = _TSComplexItem->refPoint();
_surveyRefChanging = false;
_circularSurvey->setIsInitialized(true); // prevents reinitialisation from gui
connect(_circularSurvey, &TransectStyleComplexItem::missionItemReady, this, &WimaPlaner::calcArrivalAndReturnPath);
_TSComplexItem->setIsInitialized(
true); // prevents reinitialisation from gui
connect(_TSComplexItem, &TransectStyleComplexItem::missionItemReady,
this, &WimaPlaner::calcArrivalAndReturnPath);
break;
}
}
//if (_circularSurvey == nullptr)
if ( !recalcJoinedArea() )
// if (_circularSurvey == nullptr)
if (!recalcJoinedArea())
return false;
if ( !updateMission() )
if (!updateMission())
return false;
// remove temporary file
if ( !temporaryFile.remove() ){
qWarning("WimaPlaner::loadFromFile(): not able to remove temporary file.");
if (!temporaryFile.remove()) {
qWarning(
"WimaPlaner::loadFromFile(): not able to remove temporary file.");
}
setSyncronizedWithController(false);
return true;
} else if ( fileInfo.suffix() == AppSettings::planFileExtension ){
} else if (fileInfo.suffix() == AppSettings::planFileExtension) {
_masterController->loadFromFile(filename);
return true;// might be wrong return value
return true; // might be wrong return value
} else {
errorString += QString(tr("File extension not supported.\n"));
qgcApp()->showMessage(errorMessage.arg(errorString));
......@@ -477,24 +478,23 @@ bool WimaPlaner::loadFromFile(const QString &filename)
}
}
void WimaPlaner::recalcPolygonInteractivity(int index)
{
void WimaPlaner::recalcPolygonInteractivity(int index) {
if (index >= 0 && index < _visualItems.count()) {
resetAllInteractive();
WimaArea* interactivePoly = qobject_cast<WimaArea*>(_visualItems.get(index));
WimaArea *interactivePoly =
qobject_cast<WimaArea *>(_visualItems.get(index));
if (interactivePoly != nullptr)
interactivePoly->setWimaAreaInteractive(true);
}
}
bool WimaPlaner::calcArrivalAndReturnPath()
{
bool WimaPlaner::calcArrivalAndReturnPath() {
setReadyForSync(false);
// extract old survey data
QmlObjectListModel *missionItems = _missionController->visualItems();
int surveyIndex = missionItems->indexOf(_circularSurvey);
int surveyIndex = missionItems->indexOf(_TSComplexItem);
if (surveyIndex == -1) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): no survey item");
......@@ -507,14 +507,15 @@ bool WimaPlaner::calcArrivalAndReturnPath()
// remove old arrival and return path
int size = missionItems->count();
for (int i = surveyIndex+1; i < size; i++)
_missionController->removeMissionItem(surveyIndex+1);
for (int i = surveyIndex-1; i > 1; i--)
for (int i = surveyIndex + 1; i < size; i++)
_missionController->removeMissionItem(surveyIndex + 1);
for (int i = surveyIndex - 1; i > 1; i--)
_missionController->removeMissionItem(i);
// set home position to serArea center
MissionSettingsItem* settingsItem= qobject_cast<MissionSettingsItem*>(missionItems->get(0));
if (settingsItem == nullptr){
MissionSettingsItem *settingsItem =
qobject_cast<MissionSettingsItem *>(missionItems->get(0));
if (settingsItem == nullptr) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): settingsItem == nullptr");
return false;
}
......@@ -538,8 +539,9 @@ bool WimaPlaner::calcArrivalAndReturnPath()
setCommandNeeded = true;
_missionController->insertSimpleMissionItem(_serviceArea.center(), 1);
}
SimpleMissionItem* takeOffItem = qobject_cast<SimpleMissionItem*>(missionItems->get(1));
if (takeOffItem == nullptr){
SimpleMissionItem *takeOffItem =
qobject_cast<SimpleMissionItem *>(missionItems->get(1));
if (takeOffItem == nullptr) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): takeOffItem == nullptr");
return false;
}
......@@ -547,51 +549,62 @@ bool WimaPlaner::calcArrivalAndReturnPath()
_missionController->setTakeoffCommand(*takeOffItem);
takeOffItem->setCoordinate(_serviceArea.center());
if (_circularSurvey->visualTransectPoints().size() == 0) {
if (_TSComplexItem->visualTransectPoints().size() == 0) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): survey no points.");
return false;
}
// calculate path from take off to survey
QGeoCoordinate start = _serviceArea.center();
QGeoCoordinate end = _circularSurvey->coordinate();
QGeoCoordinate end = _TSComplexItem->coordinate();
#ifdef QT_DEBUG
//if (!_visualItems.contains(&_joinedArea))
//_visualItems.append(&_joinedArea);
#endif
#ifdef QT_DEBUG
// if (!_visualItems.contains(&_joinedArea))
//_visualItems.append(&_joinedArea);
#endif
QVector<QGeoCoordinate> path;
if ( !calcShortestPath(start, end, path)) {
qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data());
if (!calcShortestPath(start, end, path)) {
qgcApp()->showMessage(QString(tr("Not able to calculate the path from "
"takeoff position to measurement area."))
.toLocal8Bit()
.data());
return false;
}
_arrivalPathLength = path.size()-1;
_arrivalPathLength = path.size() - 1;
int sequenceNumber = 0;
for (int i = 1; i < path.count()-1; i++) {
sequenceNumber = _missionController->insertSimpleMissionItem(path[i], missionItems->count()-1);
for (int i = 1; i < path.count() - 1; i++) {
sequenceNumber = _missionController->insertSimpleMissionItem(
path[i], missionItems->count() - 1);
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
}
// calculate return path
start = _circularSurvey->exitCoordinate();
start = _TSComplexItem->exitCoordinate();
end = _serviceArea.center();
path.clear();
if ( !calcShortestPath(start, end, path)) {
qgcApp()->showMessage(QString(tr("Not able to calculate the path from measurement area to landing position.")).toLocal8Bit().data());
if (!calcShortestPath(start, end, path)) {
qgcApp()->showMessage(QString(tr("Not able to calculate the path from "
"measurement area to landing position."))
.toLocal8Bit()
.data());
return false;
}
_returnPathLength = path.size()-1; // -1: fist item is last measurement point
for (int i = 1; i < path.count()-1; i++) {
sequenceNumber = _missionController->insertSimpleMissionItem(path[i], missionItems->count());
_returnPathLength =
path.size() - 1; // -1: fist item is last measurement point
for (int i = 1; i < path.count() - 1; i++) {
sequenceNumber = _missionController->insertSimpleMissionItem(
path[i], missionItems->count());
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
}
// create land position item
sequenceNumber = _missionController->insertSimpleMissionItem(_serviceArea.center(), missionItems->count());
sequenceNumber = _missionController->insertSimpleMissionItem(
_serviceArea.center(), missionItems->count());
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
SimpleMissionItem* landItem = qobject_cast<SimpleMissionItem*>(missionItems->get(missionItems->count()-1));
if (landItem == nullptr){
SimpleMissionItem *landItem = qobject_cast<SimpleMissionItem *>(
missionItems->get(missionItems->count() - 1));
if (landItem == nullptr) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): landItem == nullptr");
return false;
} else {
......@@ -600,41 +613,50 @@ bool WimaPlaner::calcArrivalAndReturnPath()
}
if (restorePlanViewIndex)
_missionController->setCurrentPlanViewIndex(missionItems->indexOf(_circularSurvey), false);
_missionController->setCurrentPlanViewIndex(
missionItems->indexOf(_TSComplexItem), false);
setSyncronizedWithControllerFalse();
setReadyForSync(true);
return true;
}
bool WimaPlaner::recalcJoinedArea()
{
bool WimaPlaner::recalcJoinedArea() {
setJoinedAreaValid(false);
// check if at least service area and measurement area are available
if (_visualItems.indexOf(&_serviceArea) == -1 || _visualItems.indexOf(&_measurementArea) == -1)
if (_visualItems.indexOf(&_serviceArea) == -1 ||
_visualItems.indexOf(&_measurementArea) == -1)
return false;
// check if area paths form simple polygons
if ( !_serviceArea.isSimplePolygon() ) {
qgcApp()->showMessage(tr("Service area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
if (!_serviceArea.isSimplePolygon()) {
qgcApp()->showMessage(
tr("Service area is self intersecting and thus not a simple polygon. "
"Only simple polygons allowed.\n"));
return false;
}
if ( !_corridor.isSimplePolygon() && _corridor.count() > 0) {
qgcApp()->showMessage(tr("Corridor is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
if (!_corridor.isSimplePolygon() && _corridor.count() > 0) {
qgcApp()->showMessage(
tr("Corridor is self intersecting and thus not a simple polygon. Only "
"simple polygons allowed.\n"));
return false;
}
if ( !_measurementArea.isSimplePolygon() ) {
qgcApp()->showMessage(tr("Measurement area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
if (!_measurementArea.isSimplePolygon()) {
qgcApp()->showMessage(
tr("Measurement area is self intersecting and thus not a simple "
"polygon. Only simple polygons allowed.\n"));
return false;
}
_joinedArea.setPath(_serviceArea.path());
_joinedArea.join(_corridor);
if ( !_joinedArea.join(_measurementArea) ) {
/*qgcApp()->showMessage(tr("Not able to join areas. Service area and measurement"
" must have a overlapping section, or be connected through a corridor."));*/
if (!_joinedArea.join(_measurementArea)) {
/*qgcApp()->showMessage(tr("Not able to join areas. Service area and
measurement"
" must have a overlapping section, or be connected
through a corridor."));*/
return false; // this happens if all areas are pairwise disjoint
}
......@@ -643,8 +665,7 @@ bool WimaPlaner::recalcJoinedArea()
return true;
}
void WimaPlaner::pushToWimaController()
{
void WimaPlaner::pushToWimaController() {
if (_wimaBridge != nullptr) {
if (!_readyForSync)
return;
......@@ -658,50 +679,48 @@ void WimaPlaner::pushToWimaController()
bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path)
{
QVector<QGeoCoordinate> &path) {
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0,0);
QPointF start2D(0, 0);
QPointF end2D;
QVector<QPointF> path2D;
toCartesian(destination, start, end2D);
bool retVal = PolygonCalculus::shortestPath( polygon2D, start2D, end2D, path2D);
bool retVal =
PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
void WimaPlaner::resetAllInteractive()
{
void WimaPlaner::resetAllInteractive() {
// Marks all areas as inactive (area.interactive == false)
int itemCount = _visualItems.count();
if (itemCount > 0){
if (itemCount > 0) {
for (int i = 0; i < itemCount; i++) {
WimaArea* iteratorPoly = qobject_cast<WimaArea*>(_visualItems.get(i));
WimaArea *iteratorPoly = qobject_cast<WimaArea *>(_visualItems.get(i));
iteratorPoly->setWimaAreaInteractive(false);
}
}
}
void WimaPlaner::setInteractive()
{
void WimaPlaner::setInteractive() {
recalcPolygonInteractivity(_currentAreaIndex);
}
/*!
* \fn WimaPlanData WimaPlaner::toPlanData()
*
* Returns a \c WimaPlanData object containing information about the current mission.
* The \c WimaPlanData object holds only the data which is relevant for the \c WimaController class.
* Should only be called if updateMission() was successful.
* Returns a \c WimaPlanData object containing information about the current
* mission. The \c WimaPlanData object holds only the data which is relevant for
* the \c WimaController class. Should only be called if updateMission() was
* successful.
*
* \sa WimaController, WimaPlanData
*/
WimaPlanData WimaPlaner::toPlanData()
{
WimaPlanData WimaPlaner::toPlanData() {
WimaPlanData planData;
// store areas
......@@ -711,13 +730,16 @@ WimaPlanData WimaPlaner::toPlanData()
planData.append(WimaJoinedAreaData(_joinedArea));
// convert mission items to mavlink commands
QObject deleteObject; // used to automatically delete content of rgMissionItems after this function call
QList<MissionItem*> rgMissionItems;
QObject deleteObject; // used to automatically delete content of
// rgMissionItems after this function call
QList<MissionItem *> rgMissionItems;
QmlObjectListModel *visualItems = _missionController->visualItems();
QmlObjectListModel visualItemsToCopy;
for (int i = _arrivalPathLength+1; i < visualItems->count()-_returnPathLength; i++)
for (unsigned long i = _arrivalPathLength + 1;
i < visualItems->count() - _returnPathLength; i++)
visualItemsToCopy.append(visualItems->get(i));
MissionController::convertToMissionItems(&visualItemsToCopy, rgMissionItems, &deleteObject);
MissionController::convertToMissionItems(&visualItemsToCopy, rgMissionItems,
&deleteObject);
// store mavlink commands
planData.append(rgMissionItems);
......@@ -725,25 +747,22 @@ WimaPlanData WimaPlaner::toPlanData()
return planData;
}
void WimaPlaner::setSyncronizedWithController(bool sync)
{
void WimaPlaner::setSyncronizedWithController(bool sync) {
if (_syncronizedWithController != sync) {
_syncronizedWithController = sync;
emit syncronizedWithControllerChanged();
}
}
void WimaPlaner::setReadyForSync(bool ready)
{
if( _readyForSync != ready) {
void WimaPlaner::setReadyForSync(bool ready) {
if (_readyForSync != ready) {
_readyForSync = ready;
emit readyForSyncChanged();
}
}
void WimaPlaner::setJoinedAreaValid(bool valid)
{
void WimaPlaner::setJoinedAreaValid(bool valid) {
if (_joinedAreaValid != valid) {
_joinedAreaValid = valid;
......@@ -751,37 +770,42 @@ void WimaPlaner::setJoinedAreaValid(bool valid)
}
}
void WimaPlaner::updateTimerSlot()
{
void WimaPlaner::updateTimerSlot() {
// General operation of this function:
// Check if parameter has changed, wait until it stops changing, update mission
// Check if parameter has changed, wait until it stops changing, update
// mission
// circular survey reference point
// if (_missionController != nullptr
// && _missionController->visualItems()->indexOf(_circularSurvey) != -1
// && _circularSurvey != nullptr)
// {
// if (_surveyRefChanging) {
// if (_circularSurvey->refPoint() == _lastSurveyRefPoint) { // is it still changing?
// calcArrivalAndReturnPath();
// _surveyRefChanging = false;
// }
// } else {
// if (_circularSurvey->refPoint() != _lastSurveyRefPoint) // does it started changing?
// _surveyRefChanging = true;
// }
// }
// if (_missionController != nullptr
// && _missionController->visualItems()->indexOf(_circularSurvey)
// != -1
// && _circularSurvey != nullptr)
// {
// if (_surveyRefChanging) {
// if (_circularSurvey->refPoint() == _lastSurveyRefPoint) { // is
// it still changing?
// calcArrivalAndReturnPath();
// _surveyRefChanging = false;
// }
// } else {
// if (_circularSurvey->refPoint() != _lastSurveyRefPoint) // does
// it started changing?
// _surveyRefChanging = true;
// }
// }
// measurementArea
if (_measurementAreaChanging) {
if (_measurementArea.path() == _lastMeasurementAreaPath) { // is it still changing?
if (_measurementArea.path() ==
_lastMeasurementAreaPath) { // is it still changing?
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
if (recalcJoinedArea() && calcArrivalAndReturnPath())
setReadyForSync(true);
_measurementAreaChanging = false;
}
} else {
if (_measurementArea.path() != _lastMeasurementAreaPath) // does it started changing?
if (_measurementArea.path() !=
_lastMeasurementAreaPath) // does it started changing?
_measurementAreaChanging = true;
}
......@@ -789,7 +813,7 @@ void WimaPlaner::updateTimerSlot()
if (_corridorChanging) {
if (_corridor.path() == _lastCorridorPath) { // is it still changing?
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
if (recalcJoinedArea() && calcArrivalAndReturnPath())
setReadyForSync(true);
_corridorChanging = false;
}
......@@ -802,80 +826,81 @@ void WimaPlaner::updateTimerSlot()
if (_serviceAreaChanging) {
if (_serviceArea.path() == _lastServiceAreaPath) { // is it still changing?
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
if (recalcJoinedArea() && calcArrivalAndReturnPath())
setReadyForSync(true);
_serviceAreaChanging = false;
}
} else {
if (_serviceArea.path() != _lastServiceAreaPath) // does it started changing?
if (_serviceArea.path() !=
_lastServiceAreaPath) // does it started changing?
_serviceAreaChanging = true;
}
// update old values
// if (_missionController != nullptr
// && _missionController->visualItems()->indexOf(_circularSurvey) != -1
// && _circularSurvey != nullptr)
// _lastSurveyRefPoint = _circularSurvey->refPoint();
// if (_missionController != nullptr
// && _missionController->visualItems()->indexOf(_circularSurvey)
// != -1
// && _circularSurvey != nullptr)
// _lastSurveyRefPoint = _circularSurvey->refPoint();
_lastMeasurementAreaPath = _measurementArea.path();
_lastCorridorPath = _corridor.path();
_lastServiceAreaPath = _serviceArea.path();
}
void WimaPlaner::setSyncronizedWithControllerFalse()
{
void WimaPlaner::setSyncronizedWithControllerFalse() {
setSyncronizedWithController(false);
}
#ifndef NDEBUG
void WimaPlaner::autoLoadMission()
{
loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/KlingenbachTest.wima");
void WimaPlaner::autoLoadMission() {
loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/"
"KlingenbachTest.wima");
pushToWimaController();
}
#endif
void WimaPlaner::startCalcArrivalAndReturnTimer()
{
void WimaPlaner::startCalcArrivalAndReturnTimer() {
_calcArrivalAndReturnPathTimer.start();
}
QJsonDocument WimaPlaner::saveToJson(FileType fileType)
{
/// This function save all areas (of WimaPlaner) and all mission items (of MissionController) to a QJsonDocument
/// @param fileType is either WimaFile or PlanFile (enum), if fileType == PlanFile only mission items are stored
QJsonDocument WimaPlaner::saveToJson(FileType fileType) {
/// This function save all areas (of WimaPlaner) and all mission items (of
/// MissionController) to a QJsonDocument
/// @param fileType is either WimaFile or PlanFile (enum), if fileType ==
/// PlanFile only mission items are stored
QJsonObject json;
if ( fileType == FileType::WimaFile ) {
if (fileType == FileType::WimaFile) {
QJsonArray jsonArray;
for (int i = 0; i < _visualItems.count(); i++) {
QJsonObject json;
WimaArea* area = qobject_cast<WimaArea*>(_visualItems.get(i));
WimaArea *area = qobject_cast<WimaArea *>(_visualItems.get(i));
if (area == nullptr) {
qWarning("WimaPlaner::saveToJson(): Internal error, area == nullptr!");
return QJsonDocument();
}
// check the type of area, create and append the JsonObject to the JsonArray once determined
WimaMeasurementArea* opArea = qobject_cast<WimaMeasurementArea*>(area);
// check the type of area, create and append the JsonObject to the
// JsonArray once determined
WimaMeasurementArea *opArea = qobject_cast<WimaMeasurementArea *>(area);
if (opArea != nullptr) {
opArea->saveToJson(json);
jsonArray.append(json);
continue;
}
WimaServiceArea* serArea = qobject_cast<WimaServiceArea*>(area);
WimaServiceArea *serArea = qobject_cast<WimaServiceArea *>(area);
if (serArea != nullptr) {
serArea->saveToJson(json);
jsonArray.append(json);
continue;
}
WimaCorridor* corridor = qobject_cast<WimaCorridor*>(area);
WimaCorridor *corridor = qobject_cast<WimaCorridor *>(area);
if (corridor != nullptr) {
corridor->saveToJson(json);
jsonArray.append(json);
......@@ -897,6 +922,3 @@ QJsonDocument WimaPlaner::saveToJson(FileType fileType)
return QJsonDocument(json);
}
src/Wima/WimaPlaner.h
View file @ 7271b853
#pragma once
#include <QObject>
#include "QGCMapPolygon.h"
#include "QmlObjectListModel.h"
#include <QObject>
#include "Geometry/WimaArea.h"
#include "Geometry/WimaAreaData.h"
#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaServiceAreaData.h"
#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaJoinedArea.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaServiceAreaData.h"
#include "WimaPlanData.h"
#include "WimaBridge.h"
#include "PlanMasterController.h"
#include "MissionController.h"
#include "SurveyComplexItem.h"
#include "SimpleMissionItem.h"
#include "MissionSettingsItem.h"
#include "JsonHelper.h"
#include "QGCApplication.h"
#include "OptimisationTools.h"
#include "Geometry/PlanimetryCalculus.h"
#include "Geometry/GeoUtilities.h"
#include "QSignalBlocker"
#define TEST_FUN 0
#if TEST_FUN
#include "TestPolygonCalculus.h"
#include "testplanimetrycalculus.h"
#endif
class MissionController;
class PlanMasterController;
class WimaBridge;
class WimaPlaner : public QObject
{
class WimaPlaner : public QObject {
Q_OBJECT
enum FileType {WimaFile, PlanFile};
enum FileType { WimaFile, PlanFile };
public:
WimaPlaner(QObject *parent = nullptr);
template<class T>
WimaPlaner(T t, QObject *parent = nullptr) = delete;
template<class T>
WimaPlaner(T t) = delete;
Q_PROPERTY(PlanMasterController* masterController READ masterController WRITE setMasterController NOTIFY masterControllerChanged)
Q_PROPERTY(MissionController* missionController READ missionController WRITE setMissionController NOTIFY missionControllerChanged)
Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged)
Q_PROPERTY(int currentPolygonIndex READ currentPolygonIndex WRITE setCurrentPolygonIndex NOTIFY currentPolygonIndexChanged)
template <class T> WimaPlaner(T t, QObject *parent = nullptr) = delete;
template <class T> WimaPlaner(T t) = delete;
Q_PROPERTY(PlanMasterController *masterController READ masterController WRITE
setMasterController NOTIFY masterControllerChanged)
Q_PROPERTY(MissionController *missionController READ missionController WRITE
setMissionController NOTIFY missionControllerChanged)
Q_PROPERTY(QmlObjectListModel *visualItems READ visualItems NOTIFY
visualItemsChanged)
Q_PROPERTY(int currentPolygonIndex READ currentPolygonIndex WRITE
setCurrentPolygonIndex NOTIFY currentPolygonIndexChanged)
Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged)
Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT)
Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT)
Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT)
Q_PROPERTY(QGeoCoordinate joinedAreaCenter READ joinedAreaCenter CONSTANT)
Q_PROPERTY(WimaBridge* wimaBridge READ wimaBridge WRITE setWimaBridge NOTIFY wimaBridgeChanged)
Q_PROPERTY(bool syncronized READ syncronizedWithController NOTIFY syncronizedWithControllerChanged)
Q_PROPERTY(WimaBridge *wimaBridge READ wimaBridge WRITE setWimaBridge NOTIFY
wimaBridgeChanged)
Q_PROPERTY(bool syncronized READ syncronizedWithController NOTIFY
syncronizedWithControllerChanged)
Q_PROPERTY(bool readyForSync READ readyForSync NOTIFY readyForSyncChanged)
// Property accessors
PlanMasterController* masterController (void) { return _masterController; }
MissionController* missionController (void) { return _missionController; }
QmlObjectListModel* visualItems (void) ;
int currentPolygonIndex (void) const { return _currentAreaIndex; }
QString currentFile (void) const { return _currentFile; }
QStringList loadNameFilters (void) const;
QStringList saveNameFilters (void) const;
QString fileExtension (void) const { return wimaFileExtension; }
QGeoCoordinate joinedAreaCenter (void) const;
WimaBridge* wimaBridge (void) { return _wimaBridge;}
PlanMasterController *masterController(void) { return _masterController; }
MissionController *missionController(void) { return _missionController; }
QmlObjectListModel *visualItems(void);
int currentPolygonIndex(void) const { return _currentAreaIndex; }
QString currentFile(void) const { return _currentFile; }
QStringList loadNameFilters(void) const;
QStringList saveNameFilters(void) const;
QString fileExtension(void) const { return wimaFileExtension; }
QGeoCoordinate joinedAreaCenter(void) const;
WimaBridge *wimaBridge(void) { return _wimaBridge; }
// Property setters
void setMasterController (PlanMasterController* masterController);
void setMissionController (MissionController* missionController);
/// Sets the integer index pointing to the current polygon. Current polygon is set interactive.
void setCurrentPolygonIndex (int index);
void setWimaBridge (WimaBridge* bridge);
void setMasterController(PlanMasterController *masterController);
void setMissionController(MissionController *missionController);
/// Sets the integer index pointing to the current polygon. Current polygon is
/// set interactive.
void setCurrentPolygonIndex(int index);
void setWimaBridge(WimaBridge *bridge);
// Property acessors
bool syncronizedWithController ();
bool readyForSync ();
bool syncronizedWithController();
bool readyForSync();
// Member Methodes
Q_INVOKABLE WimaPlaner *thisPointer();
......@@ -95,7 +80,8 @@ public:
Q_INVOKABLE void removeArea(int index);
Q_INVOKABLE bool addServiceArea();
Q_INVOKABLE bool addCorridor();
/// Remove all areas from WimaPlaner and all mission items from MissionController
/// Remove all areas from WimaPlaner and all mission items from
/// MissionController
Q_INVOKABLE void removeAll();
/// Recalculates vehicle corridor, flight path, etc.
Q_INVOKABLE bool updateMission();
......@@ -103,52 +89,57 @@ public:
Q_INVOKABLE void pushToWimaController();
Q_INVOKABLE void saveToCurrent();
Q_INVOKABLE void saveToFile(const QString& filename);
Q_INVOKABLE void saveToFile(const QString &filename);
Q_INVOKABLE bool loadFromCurrent();
Q_INVOKABLE bool loadFromFile(const QString& filename);
Q_INVOKABLE bool loadFromFile(const QString &filename);
Q_INVOKABLE void resetAllInteractive(void);
Q_INVOKABLE void setInteractive(void);
QJsonDocument saveToJson(FileType fileType);
bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path);
bool calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
// static Members
static const char* wimaFileExtension;
static const char* areaItemsName;
static const char* missionItemsName;
static const char *wimaFileExtension;
static const char *areaItemsName;
static const char *missionItemsName;
signals:
void masterControllerChanged (void);
void missionControllerChanged (void);
void visualItemsChanged (void);
void currentPolygonIndexChanged (int index);
void currentFileChanged ();
void wimaBridgeChanged ();
void syncronizedWithControllerChanged (void);
void readyForSyncChanged (void);
void masterControllerChanged(void);
void missionControllerChanged(void);
void visualItemsChanged(void);
void currentPolygonIndexChanged(int index);
void currentFileChanged();
void wimaBridgeChanged();
void syncronizedWithControllerChanged(void);
void readyForSyncChanged(void);
private slots:
void recalcPolygonInteractivity (int index);
bool calcArrivalAndReturnPath (void);
bool recalcJoinedArea ();
// called by _updateTimer::timeout signal, updates different mission parts, if parameters (e.g. survey or areas) have changed
void updateTimerSlot ();
void setSyncronizedWithControllerFalse (void);
void recalcPolygonInteractivity(int index);
bool calcArrivalAndReturnPath(void);
bool recalcJoinedArea();
// called by _updateTimer::timeout signal, updates different mission parts, if
// parameters (e.g. survey or areas) have changed
void updateTimerSlot();
void setSyncronizedWithControllerFalse(void);
#ifndef NDEBUG
void autoLoadMission (void);
void autoLoadMission(void);
#endif
void startCalcArrivalAndReturnTimer (void);
void startCalcArrivalAndReturnTimer(void);
private:
signals:
void joinedAreaValidChanged();
private:
// Member Functions
WimaPlanData toPlanData();
void setSyncronizedWithController (bool sync);
void setReadyForSync (bool ready);
void setJoinedAreaValid (bool valid);
void setSyncronizedWithController(bool sync);
void setReadyForSync(bool ready);
void setJoinedAreaValid(bool valid);
// Member Variables
PlanMasterController *_masterController;
......@@ -157,33 +148,50 @@ private:
QString _currentFile; // file for saveing
WimaBridge *_wimaBridge; // container for data exchange with WimaController
QmlObjectListModel _visualItems; // contains all visible areas
WimaJoinedArea _joinedArea; // joined area fromed by _measurementArea, _serviceArea, _corridor
WimaJoinedArea _joinedArea; // joined area fromed by _measurementArea,
// _serviceArea, _corridor
bool _joinedAreaValid;
WimaMeasurementArea _measurementArea; // measurement area
WimaServiceArea _serviceArea; // area for supplying
WimaCorridor _corridor; // corridor connecting _measurementArea and _serviceArea
int _arrivalPathLength; // the number waypoints the arrival path consists of (path from takeoff to first measurement point)
int _returnPathLength; // the number waypoints the return path consists of (path from last measurement point to land)
CircularSurveyComplexItem* _circularSurvey; // pointer to the CircularSurvey item in _missionController.visualItems()
WimaCorridor
_corridor; // corridor connecting _measurementArea and _serviceArea
unsigned long
_arrivalPathLength; // the number waypoints the arrival path consists of
// (path from takeoff to first measurement point)
unsigned long
_returnPathLength; // the number waypoints the return path consists of
// (path from last measurement point to land)
CircularSurvey *_TSComplexItem; // pointer to the CircularSurvey item in
// _missionController.visualItems()
// auto update
QTimer _updateTimer; // on this timers timeout different mission parts will be updated, if parameters (e.g. survey or areas) have changed
QGeoCoordinate _lastSurveyRefPoint; // stores the SurveyRefPoint of the previous timer call
QTimer _updateTimer; // on this timers timeout different mission parts will be
// updated, if parameters (e.g. survey or areas) have
// changed
QGeoCoordinate _lastSurveyRefPoint; // stores the SurveyRefPoint of the
// previous timer call
bool _surveyRefChanging; // true if SurveyRefPoint is changing
QVariantList _lastMeasurementAreaPath; // stores the path of _measurementArea, at the time instance of the previous timer call
bool _measurementAreaChanging; // true if the path of the _measurementArea is changing
QVariantList _lastCorridorPath; // stores the path of _corridor, at the time instance of the previous timer call
QVariantList
_lastMeasurementAreaPath; // stores the path of _measurementArea, at the
// time instance of the previous timer call
bool _measurementAreaChanging; // true if the path of the _measurementArea is
// changing
QVariantList _lastCorridorPath; // stores the path of _corridor, at the time
// instance of the previous timer call
bool _corridorChanging; // true if the path of the _corridor is changing
QVariantList _lastServiceAreaPath; // stores the path of _serviceArea, at the time instance of the previous timer call
QVariantList _lastServiceAreaPath; // stores the path of _serviceArea, at the
// time instance of the previous timer call
bool _serviceAreaChanging; // true if the path of the _serviceArea is changing
// sync stuff
bool _syncronizedWithController; // true if planData is syncronized with wimaController
bool _syncronizedWithController; // true if planData is syncronized with
// wimaController
bool _readyForSync; // gets set by updateMission and calcArrivalAndReturnPath
#ifndef NDEBUG
QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents seg. faults
QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents
// seg. faults
#endif
QTimer _calcArrivalAndReturnPathTimer;
};
src/WimaView/CircularSurveyMapVisual.qml
View file @ 7271b853
......@@ -113,10 +113,6 @@ Item {
borderColor: "black"
interiorColor: "green"
interiorOpacity: 0.5
onDragStop: {
_missionItem.comprehensiveUpdate()
}
}
// Transect lines
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment