Commit 7271b853 authored by Valentin Platzgummer's avatar Valentin Platzgummer

circular survey optimized, some bugs though

parent a22db7f4
This source diff could not be displayed because it is too large. You can view the blob instead.
......@@ -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 \
......
......@@ -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;
}
......
......@@ -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
......
......@@ -9,217 +9,257 @@
#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
{
Q_OBJECT
class TransectStyleComplexItem : public ComplexMissionItem {
Q_OBJECT
public:
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(int cameraShots READ cameraShots NOTIFY cameraShotsChanged)
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; }
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.
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; }
// 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;
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;
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(int cameraShots READ cameraShots NOTIFY cameraShotsChanged)
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; }
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.
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; }
// 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;
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;
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;
int _sequenceNumber;
QGeoCoordinate _coordinate;
QGeoCoordinate _exitCoordinate;
QGCMapPolygon _surveyAreaPolygon;
enum CoordType {
CoordTypeInterior, ///< Interior waypoint for flight path only
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
};
typedef struct {
QGeoCoordinate coord;
CoordType coordType;
} CoordInfo_t;
QVariantList _visualTransectPoints;
QList<QList<CoordInfo_t>> _transects;
QList<QList<TerrainPathQuery::PathHeightInfo_t>> _transectsPathHeightInfo;
TerrainPolyPathQuery* _terrainPolyPathQuery;
QTimer _terrainQueryTimer;
bool _ignoreRecalc;
double _complexDistance;
int _cameraShots;
double _timeBetweenShots;
double _cruiseSpeed;
CameraCalc _cameraCalc;
bool _followTerrain;
QObject* _loadedMissionItemsParent; ///< Parent for all items in _loadedMissionItems for simpler delete
QList<MissionItem*> _loadedMissionItems; ///< Mission items loaded from plan file
QMap<QString, FactMetaData*> _metaDataMap;
SettingsFact _turnAroundDistanceFact;
SettingsFact _cameraTriggerInTurnAroundFact;
SettingsFact _hoverAndCaptureFact;
SettingsFact _refly90DegreesFact;
SettingsFact _terrainAdjustToleranceFact;
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 int _terrainQueryTimeoutMsecs;
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;
QGeoCoordinate _exitCoordinate;
QGCMapPolygon _surveyAreaPolygon;
enum CoordType {
CoordTypeInterior, ///< Interior waypoint for flight path only
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
};
typedef struct {
QGeoCoordinate coord;
CoordType coordType;
} CoordInfo_t;
QVariantList _visualTransectPoints;
using Transects = QList<QList<CoordInfo_t>>;
Transects _transects;
QList<QList<TerrainPathQuery::PathHeightInfo_t>> _transectsPathHeightInfo;
TerrainPolyPathQuery *_terrainPolyPathQuery;
QTimer _terrainQueryTimer;
bool _ignoreRecalc;
double _complexDistance;
int _cameraShots;
double _timeBetweenShots;
double _cruiseSpeed;
CameraCalc _cameraCalc;
bool _followTerrain;
QObject
*_loadedMissionItemsParent; ///< Parent for all items in
///< _loadedMissionItems for simpler delete
QList<MissionItem *>
_loadedMissionItems; ///< Mission items loaded from plan file
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _turnAroundDistanceFact;
SettingsFact _cameraTriggerInTurnAroundFact;
SettingsFact _hoverAndCaptureFact;
SettingsFact _refly90DegreesFact;
SettingsFact _terrainAdjustToleranceFact;
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 int _terrainQueryTimeoutMsecs;
private slots:
void _reallyQueryTransectsPathHeightInfo(void);
void _followTerrainChanged (bool followTerrain);
void _reallyQueryTransectsPathHeightInfo(void);
void _followTerrainChanged(bool followTerrain);
protected:
bool _transectsDirty;
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);
};
......@@ -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")
......
#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
*/
#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;
};
#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
*/
# 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
};
#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
*/
#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;
};
#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;
}
}
#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}.
\sa QPointF, Circle
*/
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints, IntersectType &type, bool calcInstersect)
{
if (!circle.isNull() && ! 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
double y = originCircleL.y();
double r = circle.radius();
if (qAbs(y) > r) {
type = NoIntersection;
return false;
}
else if ( qFuzzyCompare(qFabs(y), r) ) { // tangent
double x_ori = originCircleL.x();
if (x_ori >= 0 && x_ori <= line.length()) {
if (calcInstersect) {
QPointF intersectionPt = QPointF(x_ori, 0);
rotateReference(intersectionPt, alpha);
intersectionPoints.append(intersectionPt + translationVector);
}
type = Tangent;
return true;
}
type = NoIntersection;
return false;
} 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
if (calcInstersect) {
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
}
doesIntersect = true;
}
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)
rotateReference(intersectionPt, alpha);
intersectionPoints.append(intersectionPt + translationVector); // transform (to world system) and append second intersection point
}
doesIntersect = true;
}
type = doesIntersect ? Secant : NoIntersection;
return doesIntersect ? true : false;
}
}
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.
Returns the intersection type of the two cirles \a circle1 and \a circle2.
The function assumes that the list \a intersectionPoints is empty.
\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()) {
double r1 = circle1.radius();
double r2 = circle2.radius();
double d = distance(circle1.origin(), circle2.origin());
double r = 0;
double R = 0;
if (r1 > r2) {
R = r1; // large
r = r2; // smallline1
} else {
// this branch is also choosen if r1 == r2
R = r2;
r = r1;
}
// determine intersection type
if (r + d < R) {
// this branch is also reached if d < rLarge && rSmall == 0
type = InsideNoIntersection;
return false;
} else if (qFuzzyCompare(r + d, R)) {
if (qFuzzyIsNull(d))
{
type = CirclesEqual;
return true;
}
else {
type = InsideTouching;
}
} else if (d < R) {
type = InsideIntersection;
} else if (d - r < R) {
type = OutsideIntersection;
} else if (qFuzzyCompare(d - r, R)) {
type = OutsideTouching;
} else {
type = OutsideNoIntersection;
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
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
// || 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));
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.
return true;
}
}
type = Error;
return false;
}
} // 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)
{
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));
}
}
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(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)
{
QPointFVector dummyList;
IntersectType type = NoIntersection;
return intersects(circle, line, dummyList, type, false /* calculate intersection points*/);
}
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints)
{
IntersectType type = NoIntersection;
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();
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().
\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;}
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).
*
* 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.
*
*
* \sa QPointF
*/
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
double alpha = angle(line1);
double l1 = line1.length();
QLineF line2L1 = line2;
line2L1.translate(-translationVector);
rotateReference(line2L1, -alpha);
double x1 = line2L1.x1();
double x2 = line2L1.x2();
double y1 = line2L1.y1();
double y2 = line2L1.y2();
if (x1 > x2) {
x1 = x2;
x2 = line2L1.x1();
y1 = y2;
y2 = line2L1.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;
if (qFuzzyIsNull(k)) {
if (qFuzzyIsNull(x1) && qFuzzyIsNull(y1) && qFuzzyCompare(x2, l1)){
type = LinesEqual;
return true;
}
else {
type = LinesParallel;
return false;
}
}
double d = (y1*x2-y2*x1)/dx;
xNull = -d/k;
} else {
// lines orthogonal
if (signum(y1) != signum(y2)){
xNull = x1;
}
else {
type = NoIntersection;
return false;
}
}
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}.
if (xNull >= x1 && xNull <= x2){
// determine intersection type#include "QVector3D"
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))
type = EdgeCornerIntersection;
else
type = EdgeEdgeIntersection;
} else {
type = NoIntersection;
return false;
}
} else {
type = NoIntersection;
return false;
\sa QPointF, Circle
*/
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
double y = originCircleL.y();
double r = circle.radius();
if (qAbs(y) > r) {
type = NoIntersection;
return false;
} else if (qFuzzyCompare(qFabs(y), r)) { // tangent
double x_ori = originCircleL.x();
if (x_ori >= 0 && x_ori <= line.length()) {
if (calcInstersect) {
QPointF intersectionPt = QPointF(x_ori, 0);
rotateReference(intersectionPt, alpha);
intersectionPoints.append(intersectionPt + translationVector);
}
intersectionPt = QPointF(xNull, 0); // intersection point in line1 system
rotateReference(intersectionPt, alpha);
intersectionPt += translationVector;
type = Tangent;
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.
*
* 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)
{
if (polygon.size() >= 2) {
IntersectVector intersectionTypeList;
// 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)];
interatorLine.setP1(currentVertex);
interatorLine.setP2(nextVertex);
QPointF intersectionPoint;
IntersectType type;
if ( intersects(line, interatorLine, intersectionPoint, type) ) {
intersectionList.append(intersectionPoint);
QPair<int, int> neighbours;
neighbours.first = i;
neighbours.second = PolygonCalculus::nextVertexIndex(polygon.size(), i);
neighbourList.append(neighbours);
typeList.append(type);
}
}
if (typeList.size() > 0) {
return true;
} else {
return false;
}
} else {
return false;
}
type = NoIntersection;
return false;
} 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
if (calcInstersect) {
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
}
doesIntersect = true;
}
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)
rotateReference(intersectionPt, alpha);
intersectionPoints.append(
intersectionPt +
translationVector); // transform (to world system) and append
// second intersection point
}
doesIntersect = true;
}
type = doesIntersect ? Secant : NoIntersection;
return doesIntersect ? true : 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.
*
* \sa QPair, QVector
*/
bool intersects(const QPolygonF &polygon, const QLineF &line)
{
QPointFVector dummyGeo;
QVector<QPair<int, int>> dummyNeighbour;
intersects(polygon, line, dummyGeo, dummyNeighbour);
if (dummyGeo.size() > 0)
return true;
return false;
}
void rotateReference(QLineF &line, double alpha)
{
line.setP1(rotateReturn(line.p1(), alpha));
line.setP2(rotateReturn(line.p2(), alpha));
}
QPointF rotateReturn(QPointF point, double alpha)
{
rotateReference(point, alpha);
return point;
}
QPointFVector rotateReturn(QPointFVector points, double alpha)
{
rotateReference(points, alpha);
return points;
}
QLineF rotateReturn(QLineF line, double alpha)
{
rotateReference(line, alpha);
return line;
}
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.
Returns the intersection type of the two cirles \a circle1 and \a circle2.
The function assumes that the list \a intersectionPoints is empty.
bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt)
{
IntersectType dummyType;
return intersects(line1, line2, intersectionPt, dummyType);
}
\note Returns Error if circle.isNull() returns true;
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, NeighbourVector &neighbourList)
{
IntersectVector typeList;
return intersects(polygon, line, intersectionList, neighbourList, typeList);
}
\sa Circle
*/
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());
double r = 0;
double R = 0;
if (r1 > r2) {
R = r1; // large
r = r2; // smallline1
} else {
// this branch is also choosen if r1 == r2
R = r2;
r = r1;
}
// determine intersection type
if (r + d < R) {
// this branch is also reached if d < rLarge && rSmall == 0
type = InsideNoIntersection;
return false;
} else if (qFuzzyCompare(r + d, R)) {
if (qFuzzyIsNull(d)) {
type = CirclesEqual;
return true;
} else {
type = InsideTouching;
}
} else if (d < R) {
type = InsideIntersection;
} else if (d - r < R) {
type = OutsideIntersection;
} else if (qFuzzyCompare(d - r, R)) {
type = OutsideTouching;
} else {
type = OutsideNoIntersection;
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
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
// || 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));
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.
return true;
}
type = Error;
return false;
}
} // end anonymous namespace
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, IntersectVector &typeList)
{
NeighbourVector neighbourList;
return intersects(polygon, line, intersectionList, neighbourList, typeList);
}
/*!
\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) {
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));
}
}
void rotateReference(QPointFVector &points, double alpha) {
for (int i = 0; i < points.size(); i++) {
rotateReference(points[i], alpha);
}
}
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList)
{
NeighbourVector neighbourList;
IntersectVector typeList;
return intersects(polygon, line, intersectionList, neighbourList, typeList);
}
/*!
\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);
}
/*!
\fn IntersectType intersects(const Circle &circle1, const Circle &circle2)
Returns the intersection type of the two cirles \a circle1 and \a circle2.
void rotateReferenceDegree(QPointFVector &points, double alpha) {
for (int i = 0; i < points.size(); i++) {
rotateReferenceDegree(points[i], alpha);
}
}
\note Returns Error if circle.isNull() returns true;
/*!
\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 Circle
*/
bool intersects(const Circle &circle1, const Circle &circle2)
{
IntersectType type = NoIntersection;
QPointFVector intersectionPoints;
return intersects(circle1, circle2, intersectionPoints, type, false /*calculate intersection points*/);
}
\sa QPointF, Circle
*/
bool intersects(const Circle &circle, const QLineF &line) {
QPointFVector dummyList;
IntersectType type = NoIntersection;
return intersects(circle, line, dummyList, type,
false /* calculate intersection points*/);
}
bool intersects(const Circle &circle, const QLineF &line,
QPointFVector &intersectionPoints) {
IntersectType type = NoIntersection;
return intersects(circle, line, intersectionPoints, type,
true /* calculate intersection points*/);
}
bool intersects(const Circle &circle1, const Circle &circle2, IntersectType &type)
{
QPointFVector intersectionPoints;
return intersects(circle1, circle2, intersectionPoints, type, false /*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();
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints)
{
IntersectType type;
return intersects(circle1, circle2, intersectionPoints, type);
}
return norm(dx, dy);
}
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints, IntersectType &type)
{
return intersects(circle1, circle2, intersectionPoints, type, true /*calculate intersection points*/);
}
/*!
\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().
;
\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();
double angle(const QLineF &line)
{
return angle(line.p1(), line.p2());
}
return qAtan2(dy, dx);
}
bool contains(const QLineF &line, const QPointF &point, IntersectType &type)
{
QPointF translationVector = line.p1();
double alpha = angle(line);
double l = line.length();
QPointF pointL1 = rotateReturn(point - translationVector, -alpha);
double x = pointL1.x();
double y = pointL1.y();
if ( x >= 0 && x <= l ) {
if (qFuzzyIsNull(x) || qFuzzyCompare(x, l)) {
if (qFuzzyIsNull(y))
{
type = CornerCornerIntersection;
return true;
}
} else {
if (qFuzzyIsNull(y))
{
type = EdgeCornerIntersection;
return true;
}
}
}
type = NoIntersection;
return false;
/*!
\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;
}
bool contains(const QLineF &line, const QPointF &point)
{
IntersectType dummyType;
return contains(line, point, dummyType);
}
return angle;
}
bool contains(const QPolygonF &polygon, const QPointF &point, IntersectType &type)
{
using namespace PolygonCalculus;
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) ) {
return true;
}
}
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).
*
* 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.
*
*
* \sa QPointF
*/
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
double alpha = angle(line1);
double l1 = line1.length();
QLineF line2L1 = line2;
line2L1.translate(-translationVector);
rotateReference(line2L1, -alpha);
double x1 = line2L1.x1();
double x2 = line2L1.x2();
double y1 = line2L1.y1();
double y2 = line2L1.y2();
if (x1 > x2) {
x1 = x2;
x2 = line2L1.x1();
y1 = y2;
y2 = line2L1.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;
if (qFuzzyIsNull(k)) {
if (qFuzzyIsNull(x1) && qFuzzyIsNull(y1) && qFuzzyCompare(x2, l1)) {
type = LinesEqual;
return true;
} else {
type = LinesParallel;
return false;
}
}
}
double d = (y1 * x2 - y2 * x1) / dx;
xNull = -d / k;
} else {
// lines orthogonal
if (signum(y1) != signum(y2)) {
xNull = x1;
} else {
type = NoIntersection;
return false;
}
}
if (xNull >= x1 && xNull <= x2) {
// determine intersection type#include "QVector3D"
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))
type = EdgeCornerIntersection;
else
type = EdgeEdgeIntersection;
} else {
type = NoIntersection;
return false;
}
} else {
type = NoIntersection;
return false;
}
intersectionPt = QPointF(xNull, 0); // intersection point in line1 system
rotateReference(intersectionPt, alpha);
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.
*
* 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) {
if (polygon.size() >= 2) {
IntersectVector intersectionTypeList;
// 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)];
interatorLine.setP1(currentVertex);
interatorLine.setP2(nextVertex);
QPointF intersectionPoint;
IntersectType type;
if (intersects(line, interatorLine, intersectionPoint, type)) {
intersectionList.append(intersectionPoint);
QPair<int, int> neighbours;
neighbours.first = i;
neighbours.second = PolygonCalculus::nextVertexIndex(polygon.size(), i);
neighbourList.append(neighbours);
typeList.append(type);
}
}
if (typeList.size() > 0) {
return true;
} else {
return false;
}
} else {
return false;
}
}
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(const QPointF &p)
{
return norm(p.x(), p.y());
}
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)
{
using namespace PolygonCalculus;
for (int i = 0; i < polygon.size(); i++) {
QPointF p1 = polygon[i];
int j = nextVertexIndex(polygon.size(), i);
QPointF p2 = polygon[j];
QLineF line(p1, p2);
QPointFVector lineIntersecPts;
IntersectType type;
if (intersects(circle, line, lineIntersecPts, type)) {
QPair<int, int> neigbours;
neigbours.first = i;
neigbours.second = j;
neighbourList.append(neigbours);
typeList.append(type);
intersectionPoints.append(lineIntersecPts);
}
}
if (intersectionPoints.size() > 0)
return true;
else {
return false;
}
}
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)
{
NeighbourVector neighbourList;
return intersects(circle, polygon, intersectionPoints, neighbourList, typeList);
}
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)
{
QVector<QPointFVector> intersectionPoints;
return intersects(circle, polygon, intersectionPoints);
}
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 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))
return true;
}
/*!
* \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) {
QPointFVector dummyGeo;
QVector<QPair<int, int>> dummyNeighbour;
intersects(polygon, line, dummyGeo, dummyNeighbour);
if (dummyGeo.size() > 0)
return true;
return false;
}
void rotateReference(QLineF &line, double alpha) {
line.setP1(rotateReturn(line.p1(), alpha));
line.setP2(rotateReturn(line.p2(), alpha));
}
QPointF rotateReturn(QPointF point, double alpha) {
rotateReference(point, alpha);
return point;
}
QPointFVector rotateReturn(QPointFVector points, double alpha) {
rotateReference(points, alpha);
return points;
}
QLineF rotateReturn(QLineF line, double alpha) {
rotateReference(line, alpha);
return line;
}
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) {
IntersectVector typeList;
return intersects(polygon, line, intersectionList, neighbourList, 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) {
NeighbourVector neighbourList;
IntersectVector typeList;
return intersects(polygon, line, intersectionList, neighbourList, typeList);
}
return false;
}
/*!
\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;
// bool intersectsFast(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt)
\sa Circle
*/
bool intersects(const Circle &circle1, const Circle &circle2) {
IntersectType type = NoIntersection;
QPointFVector intersectionPoints;
return intersects(circle1, circle2, intersectionPoints, type,
false /*calculate intersection points*/);
}
bool intersects(const Circle &circle1, const Circle &circle2,
IntersectType &type) {
QPointFVector intersectionPoints;
return intersects(circle1, circle2, intersectionPoints, type,
false /*calculate intersection points*/);
}
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*/);
}
;
double angle(const QLineF &line) { return angle(line.p1(), line.p2()); }
bool contains(const QLineF &line, const QPointF &point, IntersectType &type) {
QPointF translationVector = line.p1();
double alpha = angle(line);
double l = line.length();
QPointF pointL1 = rotateReturn(point - translationVector, -alpha);
double x = pointL1.x();
double y = pointL1.y();
if (x >= 0 && x <= l) {
if (qFuzzyIsNull(x) || qFuzzyCompare(x, l)) {
if (qFuzzyIsNull(y)) {
type = CornerCornerIntersection;
return true;
}
} else {
if (qFuzzyIsNull(y)) {
type = EdgeCornerIntersection;
return true;
}
}
}
type = NoIntersection;
return false;
}
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) {
using namespace PolygonCalculus;
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)) {
return true;
}
}
return false;
}
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(const QPointF &p) { return norm(p.x(), p.y()); }
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) {
using namespace PolygonCalculus;
for (int i = 0; i < polygon.size(); i++) {
QPointF p1 = polygon[i];
int j = nextVertexIndex(polygon.size(), i);
QPointF p2 = polygon[j];
QLineF line(p1, p2);
QPointFVector lineIntersecPts;
IntersectType type;
if (intersects(circle, line, lineIntersecPts, type)) {
QPair<int, int> neigbours;
neigbours.first = i;
neigbours.second = j;
neighbourList.append(neigbours);
typeList.append(type);
intersectionPoints.append(lineIntersecPts);
}
}
if (intersectionPoints.size() > 0)
return true;
else {
return false;
}
}
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) {
NeighbourVector neighbourList;
return intersects(circle, polygon, intersectionPoints, neighbourList,
typeList);
}
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) {
QVector<QPointFVector> intersectionPoints;
return intersects(circle, polygon, intersectionPoints);
}
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 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))
return true;
}
return false;
}
// 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.
*/
......@@ -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
Tangent, Secant, // Circle Line Intersetion
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);
/*!
* \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));
}
}
enum IntersectType {
InsideNoIntersection,
InsideTouching,
InsideIntersection,
OutsideIntersection,
OutsideTouching,
OutsideNoIntersection,
CirclesEqual, // Circle Circle intersection
Tangent,
Secant, // Circle Line Intersetion
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);
/*!
* \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)); }
} // namespace PlanimetryCalculus
......@@ -397,6 +397,8 @@ void SnakeThread::run() {
waypointsValid = false;
}
}
} else {
waypointsValid = false;
}
UniqueLock lk(this->pImpl->output.mutex);
......
......@@ -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,
......
#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)
{
_lastMeasurementAreaPath = _measurementArea.path();
_lastServiceAreaPath = _serviceArea.path();
_lastCorridorPath = _corridor.path();
connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::recalcPolygonInteractivity);
connect(&_updateTimer, &QTimer::timeout, this, &WimaPlaner::updateTimerSlot);
connect(this, &WimaPlaner::joinedAreaValidChanged, this, &WimaPlaner::updateMission);
_updateTimer.setInterval(300); // 300 ms means: max update time 2*300 ms
_updateTimer.start();
: 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(&_updateTimer, &QTimer::timeout, this, &WimaPlaner::updateTimerSlot);
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);
_autoLoadTimer.setSingleShot(true);
_autoLoadTimer.start(300);
// for debugging and testing purpose, remove if not needed anymore
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);
_calcArrivalAndReturnPathTimer.setInterval(100);
_calcArrivalAndReturnPathTimer.setSingleShot(true);
connect(&_calcArrivalAndReturnPathTimer, &QTimer::timeout, this,
&WimaPlaner::calcArrivalAndReturnPath);
}
QmlObjectListModel* WimaPlaner::visualItems()
{
return &_visualItems;
}
QmlObjectListModel *WimaPlaner::visualItems() { return &_visualItems; }
QStringList WimaPlaner::loadNameFilters() const
{
QStringList filters;
QStringList WimaPlaner::loadNameFilters() const {
QStringList filters;
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
tr("All Files (*.*)");
return filters;
filters << tr("Supported types (*.%1 *.%2)")
.arg(wimaFileExtension)
.arg(AppSettings::planFileExtension)
<< tr("All Files (*.*)");
return filters;
}
QStringList WimaPlaner::saveNameFilters() const
{
QStringList filters;
QStringList WimaPlaner::saveNameFilters() const {
QStringList filters;
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
return filters;
filters << tr("Supported types (*.%1 *.%2)")
.arg(wimaFileExtension)
.arg(AppSettings::planFileExtension);
return filters;
}
QGeoCoordinate WimaPlaner::joinedAreaCenter() const
{
return _joinedArea.center();
QGeoCoordinate WimaPlaner::joinedAreaCenter() const {
return _joinedArea.center();
}
void WimaPlaner::setMasterController(PlanMasterController *masterC)
{
_masterController = masterC;
emit masterControllerChanged();
void WimaPlaner::setMasterController(PlanMasterController *masterC) {
_masterController = masterC;
emit masterControllerChanged();
}
void WimaPlaner::setMissionController(MissionController *missionC)
{
_missionController = missionC;
emit missionControllerChanged();
}
void WimaPlaner::setCurrentPolygonIndex(int index)
{
if(index >= 0 && index < _visualItems.count() && index != _currentAreaIndex){
_currentAreaIndex = index;
emit currentPolygonIndexChanged(index);
}
void WimaPlaner::setMissionController(MissionController *missionC) {
_missionController = missionC;
emit missionControllerChanged();
}
void WimaPlaner::setWimaBridge(WimaBridge *bridge)
{
if (bridge != nullptr) {
_wimaBridge = bridge;
emit wimaBridgeChanged();
}
}
void WimaPlaner::setCurrentPolygonIndex(int index) {
if (index >= 0 && index < _visualItems.count() &&
index != _currentAreaIndex) {
_currentAreaIndex = index;
bool WimaPlaner::syncronizedWithController()
{
return _syncronizedWithController;
emit currentPolygonIndexChanged(index);
}
}
bool WimaPlaner::readyForSync()
{
return _readyForSync;
void WimaPlaner::setWimaBridge(WimaBridge *bridge) {
if (bridge != nullptr) {
_wimaBridge = bridge;
emit wimaBridgeChanged();
}
}
WimaPlaner *WimaPlaner::thisPointer()
{
return this;
bool WimaPlaner::syncronizedWithController() {
return _syncronizedWithController;
}
void WimaPlaner::removeArea(int index)
{
if(index >= 0 && index < _visualItems.count()){
WimaArea* area = qobject_cast<WimaArea*>(_visualItems.removeAt(index));
if ( area == nullptr) {
qWarning("WimaPlaner::removeArea(): nullptr catched, internal error.");
return;
}
area->clear();
area->borderPolygon()->clear();
bool WimaPlaner::readyForSync() { return _readyForSync; }
emit visualItemsChanged();
WimaPlaner *WimaPlaner::thisPointer() { return this; }
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.
resetAllInteractive();
_currentAreaIndex = -1;
return;
}
void WimaPlaner::removeArea(int index) {
if (index >= 0 && index < _visualItems.count()) {
WimaArea *area = qobject_cast<WimaArea *>(_visualItems.removeAt(index));
if(_currentAreaIndex >= _visualItems.count()){
setCurrentPolygonIndex(_visualItems.count() - 1);
}else{
recalcPolygonInteractivity(_currentAreaIndex);
}
}else{
qWarning("Index out of bounds!");
if (area == nullptr) {
qWarning("WimaPlaner::removeArea(): nullptr catched, internal error.");
return;
}
area->clear();
area->borderPolygon()->clear();
}
bool WimaPlaner::addMeasurementArea()
{
if (!_visualItems.contains(&_measurementArea)) {
_visualItems.append(&_measurementArea);
int newIndex = _visualItems.count()-1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
emit visualItemsChanged();
return true;
} else {
return false;
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.
resetAllInteractive();
_currentAreaIndex = -1;
return;
}
}
bool WimaPlaner::addServiceArea()
{
if (!_visualItems.contains(&_serviceArea)) {
_visualItems.append(&_serviceArea);
int newIndex = _visualItems.count()-1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
return true;
if (_currentAreaIndex >= _visualItems.count()) {
setCurrentPolygonIndex(_visualItems.count() - 1);
} else {
return false;
recalcPolygonInteractivity(_currentAreaIndex);
}
} else {
qWarning("Index out of bounds!");
}
}
bool WimaPlaner::addCorridor()
{
if (!_visualItems.contains(&_corridor)) {
_visualItems.append(&_corridor);
bool WimaPlaner::addMeasurementArea() {
if (!_visualItems.contains(&_measurementArea)) {
_visualItems.append(&_measurementArea);
int newIndex = _visualItems.count()-1;
setCurrentPolygonIndex(newIndex);
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
return true;
} else {
return false;
}
emit visualItemsChanged();
return true;
} else {
return false;
}
}
void WimaPlaner::removeAll()
{
bool changesApplied = false;
while (_visualItems.count() > 0) {
removeArea(0);
changesApplied = true;
}
bool WimaPlaner::addServiceArea() {
if (!_visualItems.contains(&_serviceArea)) {
_visualItems.append(&_serviceArea);
_missionController->removeAll();
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
_currentFile = "";
emit visualItemsChanged();
return true;
} else {
return false;
}
}
_circularSurvey = nullptr;
_surveyRefChanging = false;
bool WimaPlaner::addCorridor() {
if (!_visualItems.contains(&_corridor)) {
_visualItems.append(&_corridor);
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit currentFileChanged();
if ( changesApplied )
emit visualItemsChanged();
emit visualItemsChanged();
return true;
} else {
return false;
}
}
bool WimaPlaner::updateMission()
{
setReadyForSync(false);
void WimaPlaner::removeAll() {
bool changesApplied = false;
while (_visualItems.count() > 0) {
removeArea(0);
changesApplied = true;
}
if ( !_joinedAreaValid)
return false;
_missionController->removeAll();
_currentFile = "";
// extract old survey data
QmlObjectListModel* missionItems = _missionController->visualItems();
_TSComplexItem = nullptr;
_surveyRefChanging = false;
int surveyIndex = missionItems->indexOf(_circularSurvey);
emit currentFileChanged();
if (changesApplied)
emit visualItemsChanged();
}
// 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));
bool WimaPlaner::updateMission() {
if (_circularSurvey == nullptr){
qWarning("WimaPlaner::updateMission(): survey == nullptr");
return false;
}
setReadyForSync(false);
if (!_joinedAreaValid)
return false;
// establish connections
_circularSurvey->setRefPoint(_measurementArea.center());
_lastSurveyRefPoint = _measurementArea.center();
_surveyRefChanging = false;
_circularSurvey->setIsInitialized(true); // prevents reinitialisation from gui
connect(_circularSurvey, &TransectStyleComplexItem::missionItemReady, this, &WimaPlaner::calcArrivalAndReturnPath);
}
// extract old survey data
QmlObjectListModel *missionItems = _missionController->visualItems();
// update survey area
_circularSurvey->surveyAreaPolygon()->clear();
_circularSurvey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList());
_circularSurvey->comprehensiveUpdate();
int surveyIndex = missionItems->indexOf(_TSComplexItem);
setReadyForSync(true);
return true;
}
// create survey item if not yet present
if (surveyIndex == -1) {
_missionController->insertComplexMissionItem(
_missionController->circularSurveyComplexItemName(),
_measurementArea.center(), missionItems->count());
_TSComplexItem = qobject_cast<CircularSurvey *>(
missionItems->get(missionItems->count() - 1));
void WimaPlaner::saveToCurrent()
{
saveToFile(_currentFile);
}
void WimaPlaner::saveToFile(const QString& filename)
{
if (filename.isEmpty()) {
return;
}
QString planFilename = filename;
if (!QFileInfo(filename).fileName().contains(".")) {
planFilename += QString(".%1").arg(wimaFileExtension);
if (_TSComplexItem == nullptr) {
qWarning("WimaPlaner::updateMission(): survey == nullptr");
return false;
}
QFile file(planFilename);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
qgcApp()->showMessage(tr("Plan save error %1 : %2").arg(filename).arg(file.errorString()));
_currentFile.clear();
emit currentFileChanged();
// establish connections
_TSComplexItem->setRefPoint(_measurementArea.center());
_lastSurveyRefPoint = _measurementArea.center();
_surveyRefChanging = false;
_TSComplexItem->setIsInitialized(
true); // prevents reinitialisation from gui
connect(_TSComplexItem, &TransectStyleComplexItem::missionItemReady, this,
&WimaPlaner::calcArrivalAndReturnPath);
}
// update survey area
_TSComplexItem->surveyAreaPolygon()->clear();
_TSComplexItem->surveyAreaPolygon()->appendVertices(
_measurementArea.coordinateList());
setReadyForSync(true);
return true;
}
void WimaPlaner::saveToCurrent() { saveToFile(_currentFile); }
void WimaPlaner::saveToFile(const QString &filename) {
if (filename.isEmpty()) {
return;
}
QString planFilename = filename;
if (!QFileInfo(filename).fileName().contains(".")) {
planFilename += QString(".%1").arg(wimaFileExtension);
}
QFile file(planFilename);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
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))) {
fileType = FileType::WimaFile;
} else if (planFilename.contains(
QString(".%1").arg(AppSettings::planFileExtension))) {
fileType = FileType::PlanFile;
} else {
FileType fileType = FileType::WimaFile;
if ( planFilename.contains(QString(".%1").arg(wimaFileExtension)) ) {
fileType = FileType::WimaFile;
} else if ( planFilename.contains(QString(".%1").arg(AppSettings::planFileExtension)) ) {
fileType = FileType::PlanFile;
} else {
if ( planFilename.contains(".") ) {
qgcApp()->showMessage(tr("File format not supported"));
} else {
qgcApp()->showMessage(tr("File without file extension not accepted."));
return;
}
}
if (planFilename.contains(".")) {
qgcApp()->showMessage(tr("File format not supported"));
} else {
qgcApp()->showMessage(tr("File without file extension not accepted."));
return;
}
}
QJsonDocument saveDoc = saveToJson(fileType);
file.write(saveDoc.toJson());
if(_currentFile != planFilename) {
_currentFile = planFilename;
emit currentFileChanged();
}
QJsonDocument saveDoc = saveToJson(fileType);
file.write(saveDoc.toJson());
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
QString errorString;
QString errorMessage = tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");
bool WimaPlaner::loadFromFile(const QString &filename) {
#define debug 0
QString errorString;
QString errorMessage =
tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");
if (filename.isEmpty()) {
return false;
}
if (filename.isEmpty()) {
return false;
}
QFileInfo fileInfo(filename);
QFile file(filename);
QFileInfo fileInfo(filename);
QFile file(filename);
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
errorString = file.errorString() + QStringLiteral(" ") + filename;
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
errorString = file.errorString() + QStringLiteral(" ") + filename;
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
if(fileInfo.suffix() == wimaFileExtension) {
QJsonDocument jsonDoc;
QByteArray bytes = file.readAll();
if (fileInfo.suffix() == wimaFileExtension) {
QJsonDocument jsonDoc;
QByteArray bytes = file.readAll();
if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
QJsonObject json = jsonDoc.object();
// AreaItems
QJsonArray areaArray = json[areaItemsName].toArray();
_visualItems.clear();
int validAreaCounter = 0;
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) {
bool success = _measurementArea.loadFromJson(jsonArea, errorString);
if ( !success ) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
_lastMeasurementAreaPath = _measurementArea.path(); // prevents error messages at this point
validAreaCounter++;
_visualItems.append(&_measurementArea);
emit visualItemsChanged();
} else if ( jsonArea[WimaArea::areaTypeName] == WimaServiceArea::wimaServiceAreaName) {
bool success = _serviceArea.loadFromJson(jsonArea, errorString);
if ( !success ) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
_lastServiceAreaPath = _serviceArea.path(); // prevents error messages at this point
validAreaCounter++;
_visualItems.append(&_serviceArea);
emit visualItemsChanged();
} else if ( jsonArea[WimaArea::areaTypeName] == WimaCorridor::WimaCorridorName) {
bool success = _corridor.loadFromJson(jsonArea, errorString);
if ( !success ) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
_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));
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
} else {
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);
emit currentFileChanged();
QJsonObject missionObject = json[missionItemsName].toObject();
//qWarning() << json[missionItemsName].type();
QJsonDocument missionJsonDoc = QJsonDocument(missionObject);
// create temporary file with missionItems
QFile temporaryFile;
QString cropedFileName = filename.section("/",0,-2);
#if debug
qWarning() << cropedFileName;
#endif
QString temporaryFileName;
for (int i = 0; ; i++) {
temporaryFileName = cropedFileName + QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);
// qWarning() << temporaryFileName;
if ( !QFile::exists(temporaryFileName) ) {
temporaryFile.setFileName(temporaryFileName);
if ( temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text) ) {
break;
}
}
if ( i > 1000) {
qWarning("WimaPlaner::loadFromFile(): not able to create temporary file.");
return false;
}
}
// qWarning() << missionJsonDoc.toVariant().toString();
temporaryFile.write(missionJsonDoc.toJson());
temporaryFile.close();
// load from temporary file
_masterController->loadFromFile(temporaryFileName);
if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
QmlObjectListModel *missionItems = _missionController->visualItems();
QJsonObject json = jsonDoc.object();
// AreaItems
QJsonArray areaArray = json[areaItemsName].toArray();
_visualItems.clear();
_circularSurvey = nullptr;
for (int i = 0; i < missionItems->count(); i++) {
_circularSurvey = missionItems->value<CircularSurveyComplexItem *>(i);
if (_circularSurvey != nullptr) {
int validAreaCounter = 0;
for (int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) {
QJsonObject jsonArea = areaArray[i].toObject();
_lastSurveyRefPoint = _circularSurvey->refPoint();
_surveyRefChanging = false;
_circularSurvey->setIsInitialized(true); // prevents reinitialisation from gui
connect(_circularSurvey, &TransectStyleComplexItem::missionItemReady, this, &WimaPlaner::calcArrivalAndReturnPath);
break;
}
}
if (jsonArea.contains(WimaArea::areaTypeName) &&
jsonArea[WimaArea::areaTypeName].isString()) {
if (jsonArea[WimaArea::areaTypeName] ==
WimaMeasurementArea::WimaMeasurementAreaName) {
bool success = _measurementArea.loadFromJson(jsonArea, errorString);
//if (_circularSurvey == nullptr)
if ( !recalcJoinedArea() )
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
if ( !updateMission() )
}
_lastMeasurementAreaPath =
_measurementArea.path(); // prevents error messages at this point
validAreaCounter++;
_visualItems.append(&_measurementArea);
emit visualItemsChanged();
} else if (jsonArea[WimaArea::areaTypeName] ==
WimaServiceArea::wimaServiceAreaName) {
bool success = _serviceArea.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
_lastServiceAreaPath =
_serviceArea.path(); // prevents error messages at this point
validAreaCounter++;
_visualItems.append(&_serviceArea);
emit visualItemsChanged();
} else if (jsonArea[WimaArea::areaTypeName] ==
WimaCorridor::WimaCorridorName) {
bool success = _corridor.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
// remove temporary file
if ( !temporaryFile.remove() ){
qWarning("WimaPlaner::loadFromFile(): not able to remove temporary file.");
_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));
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
setSyncronizedWithController(false);
return true;
} else if ( fileInfo.suffix() == AppSettings::planFileExtension ){
_masterController->loadFromFile(filename);
return true;// might be wrong return value
} else {
errorString += QString(tr("File extension not supported.\n"));
qgcApp()->showMessage(errorMessage.arg(errorString));
} else {
errorString += QString(tr("Invalid or non existing entry for %s.\n")
.arg(WimaArea::areaTypeName));
return false;
}
}
}
void WimaPlaner::recalcPolygonInteractivity(int index)
{
if (index >= 0 && index < _visualItems.count()) {
resetAllInteractive();
WimaArea* interactivePoly = qobject_cast<WimaArea*>(_visualItems.get(index));
if (interactivePoly != nullptr)
interactivePoly->setWimaAreaInteractive(true);
}
}
_currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(),
fileInfo.completeBaseName().toLocal8Bit().data(),
wimaFileExtension);
bool WimaPlaner::calcArrivalAndReturnPath()
{
setReadyForSync(false);
emit currentFileChanged();
// extract old survey data
QmlObjectListModel *missionItems = _missionController->visualItems();
QJsonObject missionObject = json[missionItemsName].toObject();
int surveyIndex = missionItems->indexOf(_circularSurvey);
// qWarning() << json[missionItemsName].type();
if (surveyIndex == -1) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): no survey item");
return false;
}
QJsonDocument missionJsonDoc = QJsonDocument(missionObject);
// create temporary file with missionItems
QFile temporaryFile;
QString cropedFileName = filename.section("/", 0, -2);
#if debug
qWarning() << cropedFileName;
#endif
QString temporaryFileName;
for (int i = 0;; i++) {
temporaryFileName =
cropedFileName +
QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);
// qWarning() << temporaryFileName;
if (!QFile::exists(temporaryFileName)) {
temporaryFile.setFileName(temporaryFileName);
if (temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text)) {
break;
}
}
bool restorePlanViewIndex = false;
if (surveyIndex == _missionController->currentPlanViewIndex())
restorePlanViewIndex = true;
// 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--)
_missionController->removeMissionItem(i);
// set home position to serArea center
MissionSettingsItem* settingsItem= qobject_cast<MissionSettingsItem*>(missionItems->get(0));
if (settingsItem == nullptr){
qWarning("WimaPlaner::calcArrivalAndReturnPath(): settingsItem == nullptr");
if (i > 1000) {
qWarning(
"WimaPlaner::loadFromFile(): not able to create temporary file.");
return false;
}
}
// set altitudes, temporary measure to solve bugs
QGeoCoordinate center = _serviceArea.center();
center.setAltitude(0);
_serviceArea.setCenter(center);
center = _measurementArea.center();
center.setAltitude(0);
_measurementArea.setCenter(center);
center = _corridor.center();
center.setAltitude(0);
_corridor.setCenter(center);
// set HomePos. to serArea center
settingsItem->setCoordinate(_serviceArea.center());
// set takeoff position
bool setCommandNeeded = false;
if (missionItems->count() < 3) {
setCommandNeeded = true;
_missionController->insertSimpleMissionItem(_serviceArea.center(), 1);
}
SimpleMissionItem* takeOffItem = qobject_cast<SimpleMissionItem*>(missionItems->get(1));
if (takeOffItem == nullptr){
qWarning("WimaPlaner::calcArrivalAndReturnPath(): takeOffItem == nullptr");
return false;
}
if (setCommandNeeded)
_missionController->setTakeoffCommand(*takeOffItem);
takeOffItem->setCoordinate(_serviceArea.center());
// qWarning() << missionJsonDoc.toVariant().toString();
temporaryFile.write(missionJsonDoc.toJson());
temporaryFile.close();
if (_circularSurvey->visualTransectPoints().size() == 0) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): survey no points.");
return false;
}
// load from temporary file
_masterController->loadFromFile(temporaryFileName);
// calculate path from take off to survey
QGeoCoordinate start = _serviceArea.center();
QGeoCoordinate end = _circularSurvey->coordinate();
QmlObjectListModel *missionItems = _missionController->visualItems();
#ifdef QT_DEBUG
//if (!_visualItems.contains(&_joinedArea))
//_visualItems.append(&_joinedArea);
#endif
_TSComplexItem = nullptr;
for (int i = 0; i < missionItems->count(); i++) {
_TSComplexItem = missionItems->value<CircularSurvey *>(i);
if (_TSComplexItem != nullptr) {
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());
return false;
}
_arrivalPathLength = path.size()-1;
int sequenceNumber = 0;
for (int i = 1; i < path.count()-1; i++) {
sequenceNumber = _missionController->insertSimpleMissionItem(path[i], missionItems->count()-1);
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
_lastSurveyRefPoint = _TSComplexItem->refPoint();
_surveyRefChanging = false;
_TSComplexItem->setIsInitialized(
true); // prevents reinitialisation from gui
connect(_TSComplexItem, &TransectStyleComplexItem::missionItemReady,
this, &WimaPlaner::calcArrivalAndReturnPath);
break;
}
}
// calculate return path
start = _circularSurvey->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());
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());
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
}
// if (_circularSurvey == nullptr)
if (!recalcJoinedArea())
return false;
if (!updateMission())
return false;
// create land position item
sequenceNumber = _missionController->insertSimpleMissionItem(_serviceArea.center(), missionItems->count());
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
SimpleMissionItem* landItem = qobject_cast<SimpleMissionItem*>(missionItems->get(missionItems->count()-1));
if (landItem == nullptr){
qWarning("WimaPlaner::calcArrivalAndReturnPath(): landItem == nullptr");
return false;
} else {
if (!_missionController->setLandCommand(*landItem))
return false;
// remove temporary file
if (!temporaryFile.remove()) {
qWarning(
"WimaPlaner::loadFromFile(): not able to remove temporary file.");
}
if (restorePlanViewIndex)
_missionController->setCurrentPlanViewIndex(missionItems->indexOf(_circularSurvey), false);
setSyncronizedWithControllerFalse();
setReadyForSync(true);
setSyncronizedWithController(false);
return true;
}
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)
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"));
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"));
return false;
}
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."));*/
return false; // this happens if all areas are pairwise disjoint
}
// join service area, op area and corridor
setJoinedAreaValid(true);
return true;
}
} else if (fileInfo.suffix() == AppSettings::planFileExtension) {
_masterController->loadFromFile(filename);
return true; // might be wrong return value
} else {
errorString += QString(tr("File extension not supported.\n"));
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
}
void WimaPlaner::recalcPolygonInteractivity(int index) {
if (index >= 0 && index < _visualItems.count()) {
resetAllInteractive();
WimaArea *interactivePoly =
qobject_cast<WimaArea *>(_visualItems.get(index));
if (interactivePoly != nullptr)
interactivePoly->setWimaAreaInteractive(true);
}
}
bool WimaPlaner::calcArrivalAndReturnPath() {
setReadyForSync(false);
// extract old survey data
QmlObjectListModel *missionItems = _missionController->visualItems();
int surveyIndex = missionItems->indexOf(_TSComplexItem);
if (surveyIndex == -1) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): no survey item");
return false;
}
bool restorePlanViewIndex = false;
if (surveyIndex == _missionController->currentPlanViewIndex())
restorePlanViewIndex = true;
// 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--)
_missionController->removeMissionItem(i);
// set home position to serArea center
MissionSettingsItem *settingsItem =
qobject_cast<MissionSettingsItem *>(missionItems->get(0));
if (settingsItem == nullptr) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): settingsItem == nullptr");
return false;
}
// set altitudes, temporary measure to solve bugs
QGeoCoordinate center = _serviceArea.center();
center.setAltitude(0);
_serviceArea.setCenter(center);
center = _measurementArea.center();
center.setAltitude(0);
_measurementArea.setCenter(center);
center = _corridor.center();
center.setAltitude(0);
_corridor.setCenter(center);
// set HomePos. to serArea center
settingsItem->setCoordinate(_serviceArea.center());
// set takeoff position
bool setCommandNeeded = false;
if (missionItems->count() < 3) {
setCommandNeeded = true;
_missionController->insertSimpleMissionItem(_serviceArea.center(), 1);
}
SimpleMissionItem *takeOffItem =
qobject_cast<SimpleMissionItem *>(missionItems->get(1));
if (takeOffItem == nullptr) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): takeOffItem == nullptr");
return false;
}
if (setCommandNeeded)
_missionController->setTakeoffCommand(*takeOffItem);
takeOffItem->setCoordinate(_serviceArea.center());
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 = _TSComplexItem->coordinate();
#ifdef QT_DEBUG
// if (!_visualItems.contains(&_joinedArea))
//_visualItems.append(&_joinedArea);
#endif
void WimaPlaner::pushToWimaController()
{
if (_wimaBridge != nullptr) {
if (!_readyForSync)
return;
WimaPlanData planData = toPlanData();
(void)_wimaBridge->setWimaPlanData(planData);
setSyncronizedWithController(true);
} else {
qWarning("WimaPlaner::uploadToContainer(): no container assigned.");
}
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());
return false;
}
_arrivalPathLength = path.size() - 1;
int sequenceNumber = 0;
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 = _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());
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());
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
}
// create land position item
sequenceNumber = _missionController->insertSimpleMissionItem(
_serviceArea.center(), missionItems->count());
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
SimpleMissionItem *landItem = qobject_cast<SimpleMissionItem *>(
missionItems->get(missionItems->count() - 1));
if (landItem == nullptr) {
qWarning("WimaPlaner::calcArrivalAndReturnPath(): landItem == nullptr");
return false;
} else {
if (!_missionController->setLandCommand(*landItem))
return false;
}
if (restorePlanViewIndex)
_missionController->setCurrentPlanViewIndex(
missionItems->indexOf(_TSComplexItem), false);
setSyncronizedWithControllerFalse();
setReadyForSync(true);
return true;
}
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)
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"));
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"));
return false;
}
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."));*/
return false; // this happens if all areas are pairwise disjoint
}
// join service area, op area and corridor
setJoinedAreaValid(true);
return true;
}
void WimaPlaner::pushToWimaController() {
if (_wimaBridge != nullptr) {
if (!_readyForSync)
return;
WimaPlanData planData = toPlanData();
(void)_wimaBridge->setWimaPlanData(planData);
setSyncronizedWithController(true);
} else {
qWarning("WimaPlaner::uploadToContainer(): no container assigned.");
}
}
bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0,0);
QPointF end2D;
QVector<QPointF> path2D;
toCartesian(destination, start, end2D);
bool retVal = PolygonCalculus::shortestPath( polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
void WimaPlaner::resetAllInteractive()
{
// Marks all areas as inactive (area.interactive == false)
int itemCount = _visualItems.count();
if (itemCount > 0){
for (int i = 0; i < itemCount; i++) {
WimaArea* iteratorPoly = qobject_cast<WimaArea*>(_visualItems.get(i));
iteratorPoly->setWimaAreaInteractive(false);
}
QVector<QGeoCoordinate> &path) {
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0, 0);
QPointF end2D;
QVector<QPointF> path2D;
toCartesian(destination, start, end2D);
bool retVal =
PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
void WimaPlaner::resetAllInteractive() {
// Marks all areas as inactive (area.interactive == false)
int itemCount = _visualItems.count();
if (itemCount > 0) {
for (int i = 0; i < itemCount; i++) {
WimaArea *iteratorPoly = qobject_cast<WimaArea *>(_visualItems.get(i));
iteratorPoly->setWimaAreaInteractive(false);
}
}
}
void WimaPlaner::setInteractive()
{
recalcPolygonInteractivity(_currentAreaIndex);
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 planData;
// store areas
planData.append(WimaMeasurementAreaData(_measurementArea));
planData.append(WimaServiceAreaData(_serviceArea));
planData.append(WimaCorridorData(_corridor));
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;
QmlObjectListModel *visualItems = _missionController->visualItems();
QmlObjectListModel visualItemsToCopy;
for (int i = _arrivalPathLength+1; i < visualItems->count()-_returnPathLength; i++)
visualItemsToCopy.append(visualItems->get(i));
MissionController::convertToMissionItems(&visualItemsToCopy, rgMissionItems, &deleteObject);
// store mavlink commands
planData.append(rgMissionItems);
return planData;
}
void WimaPlaner::setSyncronizedWithController(bool sync)
{
if (_syncronizedWithController != sync) {
_syncronizedWithController = sync;
emit syncronizedWithControllerChanged();
}
WimaPlanData WimaPlaner::toPlanData() {
WimaPlanData planData;
// store areas
planData.append(WimaMeasurementAreaData(_measurementArea));
planData.append(WimaServiceAreaData(_serviceArea));
planData.append(WimaCorridorData(_corridor));
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;
QmlObjectListModel *visualItems = _missionController->visualItems();
QmlObjectListModel visualItemsToCopy;
for (unsigned long i = _arrivalPathLength + 1;
i < visualItems->count() - _returnPathLength; i++)
visualItemsToCopy.append(visualItems->get(i));
MissionController::convertToMissionItems(&visualItemsToCopy, rgMissionItems,
&deleteObject);
// store mavlink commands
planData.append(rgMissionItems);
return planData;
}
void WimaPlaner::setSyncronizedWithController(bool sync) {
if (_syncronizedWithController != sync) {
_syncronizedWithController = sync;
emit syncronizedWithControllerChanged();
}
}
void WimaPlaner::setReadyForSync(bool ready) {
if (_readyForSync != ready) {
_readyForSync = ready;
emit readyForSyncChanged();
}
}
void WimaPlaner::setJoinedAreaValid(bool valid) {
if (_joinedAreaValid != valid) {
_joinedAreaValid = valid;
emit joinedAreaValidChanged();
}
}
void WimaPlaner::updateTimerSlot() {
// General operation of this function:
// 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;
// }
// }
// measurementArea
if (_measurementAreaChanging) {
if (_measurementArea.path() ==
_lastMeasurementAreaPath) { // is it still changing?
setReadyForSync(false);
if (recalcJoinedArea() && calcArrivalAndReturnPath())
setReadyForSync(true);
_measurementAreaChanging = false;
}
} else {
if (_measurementArea.path() !=
_lastMeasurementAreaPath) // does it started changing?
_measurementAreaChanging = true;
}
// corridor
if (_corridorChanging) {
if (_corridor.path() == _lastCorridorPath) { // is it still changing?
setReadyForSync(false);
if (recalcJoinedArea() && calcArrivalAndReturnPath())
setReadyForSync(true);
_corridorChanging = false;
}
} else {
if (_corridor.path() != _lastCorridorPath) // does it started changing?
_corridorChanging = true;
}
// service area
if (_serviceAreaChanging) {
if (_serviceArea.path() == _lastServiceAreaPath) { // is it still changing?
setReadyForSync(false);
if (recalcJoinedArea() && calcArrivalAndReturnPath())
setReadyForSync(true);
_serviceAreaChanging = false;
}
} else {
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();
_lastMeasurementAreaPath = _measurementArea.path();
_lastCorridorPath = _corridor.path();
_lastServiceAreaPath = _serviceArea.path();
}
void WimaPlaner::setSyncronizedWithControllerFalse() {
setSyncronizedWithController(false);
}
void WimaPlaner::setReadyForSync(bool ready)
{
if( _readyForSync != ready) {
_readyForSync = ready;
emit readyForSyncChanged();
}
#ifndef NDEBUG
void WimaPlaner::autoLoadMission() {
loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/"
"KlingenbachTest.wima");
pushToWimaController();
}
#endif
void WimaPlaner::setJoinedAreaValid(bool valid)
{
if (_joinedAreaValid != valid) {
_joinedAreaValid = valid;
emit joinedAreaValidChanged();
}
void WimaPlaner::startCalcArrivalAndReturnTimer() {
_calcArrivalAndReturnPathTimer.start();
}
void WimaPlaner::updateTimerSlot()
{
// General operation of this function:
// 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;
// }
// }
// measurementArea
if (_measurementAreaChanging) {
if (_measurementArea.path() == _lastMeasurementAreaPath) { // is it still changing?
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
setReadyForSync(true);
_measurementAreaChanging = false;
}
} else {
if (_measurementArea.path() != _lastMeasurementAreaPath) // does it started changing?
_measurementAreaChanging = true;
}
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;
// corridor
if (_corridorChanging) {
if (_corridor.path() == _lastCorridorPath) { // is it still changing?
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
setReadyForSync(true);
_corridorChanging = false;
}
} else {
if (_corridor.path() != _lastCorridorPath) // does it started changing?
_corridorChanging = true;
}
if (fileType == FileType::WimaFile) {
QJsonArray jsonArray;
// service area
if (_serviceAreaChanging) {
if (_serviceArea.path() == _lastServiceAreaPath) { // is it still changing?
setReadyForSync(false);
if ( recalcJoinedArea() && calcArrivalAndReturnPath() )
setReadyForSync(true);
_serviceAreaChanging = false;
}
} else {
if (_serviceArea.path() != _lastServiceAreaPath) // does it started changing?
_serviceAreaChanging = true;
}
for (int i = 0; i < _visualItems.count(); i++) {
QJsonObject json;
WimaArea *area = qobject_cast<WimaArea *>(_visualItems.get(i));
// update old values
// if (_missionController != nullptr
// && _missionController->visualItems()->indexOf(_circularSurvey) != -1
// && _circularSurvey != nullptr)
// _lastSurveyRefPoint = _circularSurvey->refPoint();
if (area == nullptr) {
qWarning("WimaPlaner::saveToJson(): Internal error, area == nullptr!");
return QJsonDocument();
}
_lastMeasurementAreaPath = _measurementArea.path();
_lastCorridorPath = _corridor.path();
_lastServiceAreaPath = _serviceArea.path();
}
// 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;
}
void WimaPlaner::setSyncronizedWithControllerFalse()
{
setSyncronizedWithController(false);
}
WimaServiceArea *serArea = qobject_cast<WimaServiceArea *>(area);
if (serArea != nullptr) {
serArea->saveToJson(json);
jsonArray.append(json);
continue;
}
#ifndef NDEBUG
void WimaPlaner::autoLoadMission()
{
loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/KlingenbachTest.wima");
pushToWimaController();
}
#endif
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
QJsonObject json;
if ( fileType == FileType::WimaFile ) {
QJsonArray jsonArray;
for (int i = 0; i < _visualItems.count(); i++) {
QJsonObject json;
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);
if (opArea != nullptr) {
opArea->saveToJson(json);
jsonArray.append(json);
continue;
}
WimaServiceArea* serArea = qobject_cast<WimaServiceArea*>(area);
if (serArea != nullptr) {
serArea->saveToJson(json);
jsonArray.append(json);
continue;
}
WimaCorridor* corridor = qobject_cast<WimaCorridor*>(area);
if (corridor != nullptr) {
corridor->saveToJson(json);
jsonArray.append(json);
continue;
}
// if non of the obove branches was trigger, type must be WimaArea
area->saveToJson(json);
jsonArray.append(json);
}
WimaCorridor *corridor = qobject_cast<WimaCorridor *>(area);
if (corridor != nullptr) {
corridor->saveToJson(json);
jsonArray.append(json);
continue;
}
json[areaItemsName] = jsonArray;
json[missionItemsName] = _masterController->saveToJson().object();
return QJsonDocument(json);
} else if (fileType == FileType::PlanFile) {
return _masterController->saveToJson();
// if non of the obove branches was trigger, type must be WimaArea
area->saveToJson(json);
jsonArray.append(json);
}
return QJsonDocument(json);
}
json[areaItemsName] = jsonArray;
json[missionItemsName] = _masterController->saveToJson().object();
return QJsonDocument(json);
} else if (fileType == FileType::PlanFile) {
return _masterController->saveToJson();
}
return QJsonDocument(json);
}
#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
class MissionController;
class PlanMasterController;
class WimaBridge;
#if TEST_FUN
#include "TestPolygonCalculus.h"
#include "testplanimetrycalculus.h"
#endif
class WimaPlaner : public QObject
{
Q_OBJECT
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)
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(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;}
// 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);
// Property acessors
bool syncronizedWithController ();
bool readyForSync ();
// Member Methodes
Q_INVOKABLE WimaPlaner *thisPointer();
Q_INVOKABLE bool addMeasurementArea();
/// Removes an area from _visualItems
/// @param index Index of the area to be removed
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
Q_INVOKABLE void removeAll();
/// Recalculates vehicle corridor, flight path, etc.
Q_INVOKABLE bool updateMission();
/// Pushes the generated mission data to the wimaController.
Q_INVOKABLE void pushToWimaController();
Q_INVOKABLE void saveToCurrent();
Q_INVOKABLE void saveToFile(const QString& filename);
Q_INVOKABLE bool loadFromCurrent();
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);
// static Members
static const char* wimaFileExtension;
static const char* areaItemsName;
static const char* missionItemsName;
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)
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(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; }
// 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);
// Property acessors
bool syncronizedWithController();
bool readyForSync();
// Member Methodes
Q_INVOKABLE WimaPlaner *thisPointer();
Q_INVOKABLE bool addMeasurementArea();
/// Removes an area from _visualItems
/// @param index Index of the area to be removed
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
Q_INVOKABLE void removeAll();
/// Recalculates vehicle corridor, flight path, etc.
Q_INVOKABLE bool updateMission();
/// Pushes the generated mission data to the wimaController.
Q_INVOKABLE void pushToWimaController();
Q_INVOKABLE void saveToCurrent();
Q_INVOKABLE void saveToFile(const QString &filename);
Q_INVOKABLE bool loadFromCurrent();
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);
// static Members
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();
void joinedAreaValidChanged();
private:
// Member Functions
WimaPlanData toPlanData();
void setSyncronizedWithController (bool sync);
void setReadyForSync (bool ready);
void setJoinedAreaValid (bool valid);
// Member Variables
PlanMasterController *_masterController;
MissionController *_missionController;
int _currentAreaIndex;
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
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()
// 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
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
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
bool _serviceAreaChanging; // true if the path of the _serviceArea is changing
// sync stuff
bool _syncronizedWithController; // true if planData is syncronized with wimaController
bool _readyForSync; // gets set by updateMission and calcArrivalAndReturnPath
// Member Functions
WimaPlanData toPlanData();
void setSyncronizedWithController(bool sync);
void setReadyForSync(bool ready);
void setJoinedAreaValid(bool valid);
// Member Variables
PlanMasterController *_masterController;
MissionController *_missionController;
int _currentAreaIndex;
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
bool _joinedAreaValid;
WimaMeasurementArea _measurementArea; // measurement area
WimaServiceArea _serviceArea; // area for supplying
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
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
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
bool _serviceAreaChanging; // true if the path of the _serviceArea is changing
// sync stuff
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;
QTimer _calcArrivalAndReturnPathTimer;
};
......@@ -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