Skip to content

Q
qgroundcontrol
Commit 0877c5b7 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

clearup temp commit 2

parent 458ab210
Show whitespace changes
Inline Side-by-side
Showing with 67 additions and 18465 deletions
qgroundcontrol.pro
View file @ 0877c5b7
......@@ -416,7 +416,6 @@ INCLUDEPATH += \
src/QtLocationPlugin \
src/QtLocationPlugin/QMLControl \
src/Settings \
src/Wima/Snake \
src/Wima \
src/Terrain \
src/Vehicle \
......@@ -447,74 +446,58 @@ contains (DEFINES, QGC_ENABLE_PAIRING) {
HEADERS += \
src/QmlControls/QmlUnitsConversion.h \
src/Vehicle/VehicleEscStatusFactGroup.h \
src/Wima/CircularSurvey.h \
src/Wima/GenericSingelton.h \
src/Wima/Geometry/GenericCircle.h \
src/Wima/RoutingThread.h \
src/Wima/Snake/CircularGenerator.h \
src/Wima/Snake/GeneratorBase.h \
src/Wima/Snake/LinearGenerator.h \
src/Wima/Snake/clipper/clipper.hpp \
src/Wima/Snake/mapbox/feature.hpp \
src/Wima/Snake/mapbox/geometry.hpp \
src/Wima/Snake/mapbox/geometry/box.hpp \
src/Wima/Snake/mapbox/geometry/empty.hpp \
src/Wima/Snake/mapbox/geometry/envelope.hpp \
src/Wima/Snake/mapbox/geometry/for_each_point.hpp \
src/Wima/Snake/mapbox/geometry/geometry.hpp \
src/Wima/Snake/mapbox/geometry/line_string.hpp \
src/Wima/Snake/mapbox/geometry/multi_line_string.hpp \
src/Wima/Snake/mapbox/geometry/multi_point.hpp \
src/Wima/Snake/mapbox/geometry/multi_polygon.hpp \
src/Wima/Snake/mapbox/geometry/point.hpp \
src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp \
src/Wima/Snake/mapbox/geometry/polygon.hpp \
src/Wima/Snake/mapbox/geometry_io.hpp \
src/Wima/Snake/mapbox/optional.hpp \
src/Wima/Snake/mapbox/polylabel.hpp \
src/Wima/Snake/mapbox/recursive_wrapper.hpp \
src/Wima/Snake/mapbox/variant.hpp \
src/Wima/Snake/mapbox/variant_io.hpp \
src/Wima/Snake/snake.h \
src/Wima/Geometry/GenericPolygon.h \
src/Wima/Geometry/GenericPolygonArray.h \
src/Wima/Geometry/GeoPoint3D.h \
src/Wima/Snake/NemoInterface.h \
src/Wima/Snake/QNemoHeartbeat.h \
src/Wima/Snake/QNemoProgress.h \
src/Wima/Snake/QNemoProgress.h \
src/Wima/Snake/SnakeTile.h \
src/Wima/Snake/SnakeTileLocal.h \
src/Wima/Snake/SnakeTiles.h \
src/Wima/Snake/SnakeTilesLocal.h \
src/Wima/Utils.h \
src/Wima/WimaBridge.h \
src/Wima/WimaStateMachine.h \
src/Wima/call_once.h \
src/RouteMissionItem/RouteComplexItem.h \
src/RouteMissionItem/GenericSingelton.h \
src/RouteMissionItem/geometry/GenericCircle.h \
src/RouteMissionItem/RoutingThread.h \
src/RouteMissionItem/CircularGenerator.h \
src/RouteMissionItem/GeneratorBase.h \
src/RouteMissionItem/LinearGenerator.h \
src/RouteMissionItem/geometry/clipper/clipper.hpp \
src/RouteMissionItem/geometry/mapbox/feature.hpp \
src/RouteMissionItem/geometry/mapbox/geometry.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/box.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/empty.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/envelope.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/for_each_point.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/geometry.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/line_string.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/multi_line_string.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/multi_point.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/multi_polygon.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/point.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/point_arithmetic.hpp \
src/RouteMissionItem/geometry/mapbox/geometry/polygon.hpp \
src/RouteMissionItem/geometry/mapbox/geometry_io.hpp \
src/RouteMissionItem/geometry/mapbox/optional.hpp \
src/RouteMissionItem/geometry/mapbox/polylabel.hpp \
src/RouteMissionItem/geometry/mapbox/recursive_wrapper.hpp \
src/RouteMissionItem/geometry/mapbox/variant.hpp \
src/RouteMissionItem/geometry/mapbox/variant_io.hpp \
src/RouteMissionItem/geometry/snake.h \
src/RouteMissionItem/geometry/GenericPolygon.h \
src/RouteMissionItem/geometry/GenericPolygonArray.h \
src/RouteMissionItem/geometry/GeoPoint3D.h \
src/RouteMissionItem/NemoInterface.h \
src/RouteMissionItem/nemo_interface/QNemoHeartbeat.h \
src/RouteMissionItem/nemo_interface/QNemoProgress.h \
src/RouteMissionItem/nemo_interface/QNemoProgress.h \
src/RouteMissionItem/nemo_interface/SnakeTile.h \
src/RouteMissionItem/nemo_interface/SnakeTileLocal.h \
src/RouteMissionItem/nemo_interface/SnakeTiles.h \
src/RouteMissionItem/nemo_interface/SnakeTilesLocal.h \
src/RouteMissionItem/call_once.h \
src/api/QGCCorePlugin.h \
src/api/QGCOptions.h \
src/api/QGCSettings.h \
src/api/QmlComponentInfo.h \
src/GPS/Drivers/src/base_station.h \
src/Wima/Geometry/WimaArea.h \
src/Wima/Geometry/WimaServiceArea.h \
src/Wima/Geometry/WimaTrackerPolyline.h \
src/Wima/WimaPlaner.h \
src/Wima/Geometry/WimaMeasurementArea.h \
src/Wima/Geometry/WimaCorridor.h \
src/Wima/Geometry/WimaAreaData.h \
src/Wima/Geometry/WimaServiceAreaData.h \
src/Wima/Geometry/WimaCorridorData.h \
src/Wima/Geometry/WimaMeasurementAreaData.h \
src/Wima/WimaPlanData.h \
src/Wima/Geometry/WimaJoinedArea.h \
src/Wima/Geometry/WimaJoinedAreaData.h \
src/Wima/Geometry/PlanimetryCalculus.h \
src/Wima/Geometry/PolygonCalculus.h \
src/Wima/OptimisationTools.h \
src/Wima/Geometry/GeoUtilities.h \
src/Wima/Geometry/TestPolygonCalculus.h \
src/Wima/Geometry/testplanimetrycalculus.h \
src/RouteMissionItem/geometry/WimaArea.h \
src/RouteMissionItem/geometry/WimaServiceArea.h \
src/RouteMissionItem/geometry/WimaMeasurementArea.h \
src/RouteMissionItem/geometry/PlanimetryCalculus.h \
src/RouteMissionItem/geometry/PolygonCalculus.h \
src/RouteMissionItem/OptimisationTools.h \
src/Settings/WimaSettings.h \
src/comm/ros_bridge/include/RosBridgeClient.h \
src/comm/ros_bridge/include/com_private.h \
......@@ -540,25 +523,23 @@ contains (DEFINES, QGC_ENABLE_PAIRING) {
SOURCES += \
src/Vehicle/VehicleEscStatusFactGroup.cc \
src/Wima/WimaStateMachine.cpp \
src/RouteMissionItem/AreaData.cc \
src/api/QGCCorePlugin.cc \
src/api/QGCOptions.cc \
src/api/QGCSettings.cc \
src/api/QmlComponentInfo.cc \
src/Wima/CircularSurvey.cc \
src/Wima/GenericSingelton.cpp \
src/Wima/RoutingThread.cpp \
src/Wima/Snake/CircularGenerator.cpp \
src/Wima/Snake/GeneratorBase.cc \
src/Wima/Snake/LinearGenerator.cpp \
src/Wima/Snake/clipper/clipper.cpp \
src/Wima/Snake/snake.cpp \
src/Wima/Geometry/GeoPoint3D.cpp \
src/Wima/Snake/NemoInterface.cpp \
src/Wima/Snake/QNemoProgress.cc \
src/Wima/Snake/SnakeTile.cpp \
src/Wima/Utils.cpp \
src/Wima/WimaBridge.cc \
src/RouteMissionItem/RouteComplexItem.cc \
src/RouteMissionItem/GenericSingelton.cpp \
src/RouteMissionItem/RoutingThread.cpp \
src/RouteMissionItem/CircularGenerator.cpp \
src/RouteMissionItem/GeneratorBase.cc \
src/RouteMissionItem/LinearGenerator.cpp \
src/RouteMissionItem/geometry/clipper/clipper.cpp \
src/RouteMissionItem/geometry/snake.cpp \
src/RouteMissionItem/geometry/GeoPoint3D.cpp \
src/RouteMissionItem/NemoInterface.cpp \
src/RouteMissionItem/nemo_interface/QNemoProgress.cc \
src/RouteMissionItem/nemo_interface/SnakeTile.cpp \
src/comm/ros_bridge/include/RosBridgeClient.cpp \
src/comm/ros_bridge/include/com_private.cpp \
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \
......@@ -573,25 +554,12 @@ SOURCES += \
src/comm/ros_bridge/include/server.cpp \
src/comm/ros_bridge/include/topic_publisher.cpp \
src/comm/ros_bridge/include/topic_subscriber.cpp \
src/Wima/Geometry/WimaArea.cc \
src/Wima/Geometry/WimaServiceArea.cc \
src/Wima/Geometry/WimaTrackerPolyline.cc \
src/Wima/WimaPlaner.cc \
src/Wima/Geometry/WimaMeasurementArea.cc \
src/Wima/Geometry/WimaCorridor.cc \
src/Wima/Geometry/WimaAreaData.cc \
src/Wima/Geometry/WimaServiceAreaData.cc \
src/Wima/Geometry/WimaCorridorData.cpp \
src/Wima/WimaPlanData.cc \
src/Wima/Geometry/WimaMeasurementAreaData.cc \
src/Wima/Geometry/WimaJoinedArea.cc \
src/Wima/Geometry/WimaJoinedAreaData.cc \
src/Wima/Geometry/PlanimetryCalculus.cc \
src/Wima/OptimisationTools.cc \
src/Wima/Geometry/GeoUtilities.cc \
src/Wima/Geometry/PolygonCalculus.cc \
src/Wima/Geometry/TestPolygonCalculus.cpp \
src/Wima/Geometry/testplanimetrycalculus.cpp \
src/RouteMissionItem/geometry/WimaArea.cc \
src/RouteMissionItem/geometry/WimaServiceArea.cc \
src/RouteMissionItem/geometry/WimaMeasurementArea.cc \
src/RouteMissionItem/geometry/PlanimetryCalculus.cc \
src/RouteMissionItem/OptimisationTools.cc \
src/RouteMissionItem/geometry/PolygonCalculus.cc \
src/Settings/WimaSettings.cc \
src/comm/ros_bridge/src/ros_bridge.cpp
......@@ -1586,9 +1554,4 @@ contains (CONFIG, QGC_DISABLE_INSTALLER_SETUP) {
}
DISTFILES += \
src/Wima/Routing/json/CircularGenerator.SettingsGroup.json \
src/Wima/Snake/json/LinearGenerator.SettingsGroup.json \
src/Wima/json/CircularSurvey.SettingsGroup.json \
src/WimaView/WimaMeasurementAreaEditor.qml \
src/Settings/Wima.SettingsGroup.json \
src/QmlControls/QGroundControl/Specific/qmldir
qgroundcontrol.qrc
View file @ 0877c5b7
......@@ -289,7 +289,6 @@
<file alias="Wima/WimaServiceAreaEditor.qml">src/WimaView/WimaServiceAreaEditor.qml</file>
<file alias="Wima/WimaServiceAreaMapVisual.qml">src/WimaView/WimaServiceAreaMapVisual.qml</file>
<file alias="Wima/WimaToolBar.qml">src/WimaView/WimaToolBar.qml</file>
<file alias="WimaView.qml">src/WimaView/WimaView.qml</file>
<file alias="SetupParameterEditor.qml">src/VehicleSetup/SetupParameterEditor.qml</file>
<file alias="SetupView.qml">src/VehicleSetup/SetupView.qml</file>
<file alias="SimpleItemEditor.qml">src/PlanView/SimpleItemEditor.qml</file>
......
src/QGCApplication.cc
View file @ 0877c5b7
......@@ -112,8 +112,6 @@
#include "Wima/Snake/CircularGenerator.h"
#include "Wima/Snake/LinearGenerator.h"
#include "Wima/Snake/NemoInterface.h"
#include "Wima/WimaController.h"
#include "Wima/WimaPlaner.h"
#if defined(QGC_ENABLE_PAIRING)
#include "PairingManager.h"
......@@ -596,8 +594,6 @@ void QGCApplication::_initCommon()
#endif
// Wima
qmlRegisterType<WimaController>("Wima", 1, 0, "WimaController");
qmlRegisterType<WimaPlaner>("Wima", 1, 0, "WimaPlaner");
qmlRegisterType<NemoInterface>("Wima", 1, 0, "NemoInterface");
qmlRegisterInterface<routing::GeneratorBase>("GeneratorBase");
qmlRegisterType<routing::CircularGenerator>("Wima", 1, 0,
......
src/QmlControls/FlightPathSegment.h
View file @ 0877c5b7
......@@ -35,7 +35,7 @@ public:
Q_PROPERTY(double distanceBetween MEMBER _distanceBetween NOTIFY distanceBetweenChanged)
Q_PROPERTY(double finalDistanceBetween MEMBER _finalDistanceBetween NOTIFY finalDistanceBetweenChanged)
Q_PROPERTY(double totalDistance MEMBER _totalDistance NOTIFY totalDistanceChanged)
Q_PROPERTY(bool terrainCollision MEMBER _terrainCollision NOTIFY terrainCollisionChanged);
Q_PROPERTY(bool terrainCollision MEMBER _terrainCollision NOTIFY terrainCollisionChanged)
QGeoCoordinate coordinate1 (void) const { return _coord1; }
QGeoCoordinate coordinate2 (void) const { return _coord2; }
......
src/Wima/.directory deleted 100644 → 0
View file @ 458ab210
[Dolphin]
Timestamp=2020,7,8,11,13,28
Version=4
ViewMode=1
src/Wima/CircularSurvey.cc deleted 100644 → 0
View file @ 458ab210
#include "CircularSurvey.h"
#include "RoutingThread.h"
// QGC
#include "JsonHelper.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
// Wima
#include "snake.h"
#define CLIPPER_SCALE 1000000
#include "clipper/clipper.hpp"
#include "Geometry/GenericCircle.h"
#include "Snake/SnakeTile.h"
// boost
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
#include "CircularGenerator.h"
#include "LinearGenerator.h"
// ToDo: Check what happened to _transectsDirty
QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog")
template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value);
}
const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::jsonComplexItemTypeValue = "CircularSurvey";
const char *CircularSurvey::variantName = "Variant";
const QString CircularSurvey::name(tr("Circular Survey"));
CircularSurvey::CircularSurvey(PlanMasterController *masterController,
bool flyView, const QString &kmlOrShpFile,
QObject *parent)
: TransectStyleComplexItem(masterController, flyView, settingsGroup,
parent),
_state(STATE::IDLE),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)),
_variant(settingsGroup, _metaDataMap[variantName]),
_areaData(std::make_shared<AreaData>()),
_pWorker(std::make_unique<RoutingThread>()) {
Q_UNUSED(kmlOrShpFile)
_editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";
// Connect facts.
connect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
// Connect worker.
connect(this->_pWorker.get(), &RoutingThread::result, this,
&CircularSurvey::_setTransects);
connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this,
&CircularSurvey::calculatingChanged);
// Register Generators.
auto lg = std::make_shared<routing::LinearGenerator>(this->_areaData);
registerGenerator(lg->name(), lg);
auto cg = std::make_shared<routing::CircularGenerator>(this->_areaData);
registerGenerator(cg->name(), cg);
}
CircularSurvey::~CircularSurvey() {}
void CircularSurvey::reverse() {
this->_setState(STATE::REVERT_PATH);
this->_rebuildTransects();
}
void CircularSurvey::setPlanData(const AreaData &d) {
*this->_areaData = d;
}
const AreaData &CircularSurvey::planData() const {
return *this->_areaData;
}
AreaData &CircularSurvey::planData() { return *this->_areaData; }
QStringList CircularSurvey::variantNames() const { return _variantNames; }
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},
{variantName, QJsonValue::Double, false},
};
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, sequenceNumber, errorString)) {
_ignoreRecalc = false;
return false;
}
_variant.setRawValue(complexObject[variantName].toInt());
_ignoreRecalc = false;
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] =
jsonComplexItemTypeValue;
saveObject[variantName] = double(_variant.rawValue().toUInt());
// Polygon shape
_surveyAreaPolygon.saveToJson(saveObject);
planItems.append(saveObject);
}
bool CircularSurvey::specifiesCoordinate() const {
return _transects.count() > 0 ? _transects.first().count() > 0 : false;
}
bool CircularSurvey::_switchToGenerator(
const CircularSurvey::PtrGenerator &newG) {
if (this->_pGenerator != newG) {
if (this->_pGenerator != nullptr) {
disconnect(this->_pGenerator.get(),
&routing::GeneratorBase::generatorChanged, this,
&CircularSurvey::_rebuildTransects);
}
this->_pGenerator = newG;
connect(this->_pGenerator.get(), &routing::GeneratorBase::generatorChanged,
this, &CircularSurvey::_rebuildTransects);
emit generatorChanged();
this->_setState(STATE::IDLE);
_rebuildTransects();
return true;
} else {
return false;
}
}
void CircularSurvey::_setState(CircularSurvey::STATE state) {
if (this->_state != state) {
auto oldState = this->_state;
this->_state = state;
if (_calculating(oldState) != _calculating(state)) {
emit calculatingChanged();
}
}
}
bool CircularSurvey::_calculating(CircularSurvey::STATE state) const {
return state == STATE::ROUTING;
}
void CircularSurvey::_changeVariant() {
this->_setState(STATE::CHANGE_VARIANT);
this->_rebuildTransects();
}
bool CircularSurvey::_updateWorker() {
// Reset data.
this->_transects.clear();
this->_variantVector.clear();
this->_variantNames.clear();
emit variantNamesChanged();
if (this->_areaData->isValid()) {
// Prepare data.
auto origin = this->_areaData->origin();
origin.setAltitude(0);
if (!origin.isValid()) {
qCDebug(CircularSurveyLog)
<< "_updateWorker(): origin invalid." << origin;
return false;
}
// Convert safe area.
auto geoSafeArea = this->_areaData->joinedArea().coordinateList();
if (!(geoSafeArea.size() >= 3)) {
qCDebug(CircularSurveyLog)
<< "_updateWorker(): safe area invalid." << geoSafeArea;
return false;
}
for (auto &v : geoSafeArea) {
if (v.isValid()) {
v.setAltitude(0);
} else {
qCDebug(CircularSurveyLog)
<< "_updateWorker(): safe area contains invalid coordinate."
<< geoSafeArea;
return false;
}
}
// Routing par.
RoutingParameter par;
par.numSolutions = 5;
auto &safeAreaENU = par.safeArea;
snake::areaToEnu(origin, geoSafeArea, safeAreaENU);
// Create generator.
if (this->_pGenerator) {
routing::GeneratorBase::Generator g; // Transect generator.
if (this->_pGenerator->get(g)) {
// Start/Restart routing worker.
this->_pWorker->route(par, g);
return true;
} else {
qCDebug(CircularSurveyLog)
<< "_updateWorker(): generator creation failed.";
return false;
}
} else {
qCDebug(CircularSurveyLog)
<< "_updateWorker(): pGenerator == nullptr, number of registered "
"generators: "
<< this->_generatorList.size();
return false;
}
} else {
qCDebug(CircularSurveyLog) << "_updateWorker(): plan data invalid.";
return false;
}
}
void CircularSurvey::_changeVariantWorker() {
auto variant = this->_variant.rawValue().toUInt();
// Find old variant and run. Old run corresponts with empty list.
std::size_t old_variant = std::numeric_limits<std::size_t>::max();
for (std::size_t i = 0; i < std::size_t(this->_variantVector.size()); ++i) {
const auto &variantCoordinates = this->_variantVector.at(i);
if (variantCoordinates.isEmpty()) {
old_variant = i;
break;
}
}
// Swap route.
if (variant != old_variant) {
// Swap in new variant.
if (variant < std::size_t(this->_variantVector.size())) {
if (old_variant != std::numeric_limits<std::size_t>::max()) {
// this->_transects containes a route, swap it back to
// this->_solutionVector
auto &oldVariantCoordinates = this->_variantVector[old_variant];
oldVariantCoordinates.swap(this->_transects);
}
auto &newVariantCoordinates = this->_variantVector[variant];
this->_transects.swap(newVariantCoordinates);
} else { // error
qCDebug(CircularSurveyLog)
<< "Variant out of bounds (variant =" << variant << ").";
qCDebug(CircularSurveyLog) << "Resetting variant to zero.";
disconnect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
this->_variant.setCookedValue(QVariant(0));
connect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
if (this->_variantVector.size() > 0) {
this->_changeVariantWorker();
}
}
}
}
void CircularSurvey::_reverseWorker() {
if (this->_transects.size() > 0) {
auto &t = this->_transects.front();
std::reverse(t.begin(), t.end());
}
}
double CircularSurvey::timeBetweenShots() { return 0; }
QString CircularSurvey::commandDescription() const {
return tr("Circular Survey");
}
QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString CircularSurvey::abbreviation() const { return tr("C.S."); }
TransectStyleComplexItem::ReadyForSaveState
CircularSurvey::readyForSaveState() const {
if (TransectStyleComplexItem::readyForSaveState() ==
TransectStyleComplexItem::ReadyForSaveState::ReadyForSave) {
if (this->_state == STATE::IDLE) {
return ReadyForSaveState::ReadyForSave;
} else {
return ReadyForSaveState::NotReadyForSaveData;
}
} else {
return TransectStyleComplexItem::readyForSaveState();
}
}
double CircularSurvey::additionalTimeDelay() const { return 0; }
QString CircularSurvey::patternName() const { return name; }
bool CircularSurvey::registerGenerator(
const QString &name, std::shared_ptr<routing::GeneratorBase> g) {
if (name.isEmpty()) {
qCDebug(CircularSurveyLog) << "registerGenerator(): empty name string.";
return false;
}
if (!g) {
qCDebug(CircularSurveyLog) << "registerGenerator(): empty generator.";
return false;
}
if (this->_generatorNameList.contains(name)) {
qCDebug(CircularSurveyLog) << "registerGenerator(): generator "
"already registered.";
return false;
} else {
this->_generatorNameList.push_back(name);
this->_generatorList.push_back(g);
if (this->_generatorList.size() == 1) {
_switchToGenerator(g);
}
emit generatorNameListChanged();
return true;
}
}
bool CircularSurvey::unregisterGenerator(const QString &name) {
auto index = this->_generatorNameList.indexOf(name);
if (index >= 0) {
// Is this the current generator?
const auto &g = this->_generatorList.at(index);
if (g == this->_pGenerator) {
if (index > 0) {
_switchToGenerator(this->_generatorList.at(index - 1));
} else {
_switchToGenerator(nullptr);
qCDebug(CircularSurveyLog)
<< "unregisterGenerator(): last generator unregistered.";
}
}
this->_generatorNameList.removeAt(index);
this->_generatorList.removeAt(index);
emit generatorNameListChanged();
return true;
} else {
qCDebug(CircularSurveyLog)
<< "unregisterGenerator(): generator " << name << " not registered.";
return false;
}
}
bool CircularSurvey::unregisterGenerator(int index) {
if (index > 0 && index < this->_generatorNameList.size()) {
return unregisterGenerator(this->_generatorNameList.at(index));
} else {
qCDebug(CircularSurveyLog) << "unregisterGenerator(): index (" << index
<< ") out"
"of bounds ( "
<< this->_generatorList.size() << " ).";
return false;
}
}
bool CircularSurvey::switchToGenerator(const QString &name) {
auto index = this->_generatorNameList.indexOf(name);
if (index >= 0) {
_switchToGenerator(this->_generatorList.at(index));
return true;
} else {
qCDebug(CircularSurveyLog)
<< "switchToGenerator(): generator " << name << " not registered.";
return false;
}
}
bool CircularSurvey::switchToGenerator(int index) {
if (index >= 0) {
_switchToGenerator(this->_generatorList.at(index));
return true;
} else {
qCDebug(CircularSurveyLog) << "unregisterGenerator(): index (" << index
<< ") out"
"of bounds ( "
<< this->_generatorNameList.size() << " ).";
return false;
}
}
QStringList CircularSurvey::generatorNameList() {
return this->_generatorNameList;
}
routing::GeneratorBase *CircularSurvey::generator() {
return _pGenerator.get();
}
int CircularSurvey::generatorIndex() {
return this->_generatorList.indexOf(this->_pGenerator);
}
void CircularSurvey::_rebuildTransectsPhase1(void) {
auto start = std::chrono::high_resolution_clock::now();
switch (this->_state) {
case STATE::SKIPP:
qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: skipp.";
this->_setState(STATE::IDLE);
break;
case STATE::CHANGE_VARIANT:
qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: variant change.";
this->_changeVariantWorker();
this->_setState(STATE::IDLE);
break;
case STATE::REVERT_PATH:
qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: reverse.";
this->_reverseWorker();
this->_setState(STATE::IDLE);
break;
case STATE::IDLE:
case STATE::ROUTING:
this->_setState(STATE::ROUTING);
qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: update.";
if (!this->_updateWorker()) {
this->_setState(STATE::IDLE);
}
break;
}
qCDebug(CircularSurveyLog)
<< "rebuildTransectsPhase1(): "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
}
// no cameraShots in Circular Survey, add if desired
void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
// Store solutions.
auto ori = this->_areaData->origin();
ori.setAltitude(0);
const auto &transectsENU = pRoute->transects;
QVector<Variant> variantVector;
const auto nSolutions = pRoute->solutionVector.size();
for (std::size_t j = 0; j < nSolutions; ++j) {
Variant var{QList<CoordInfo_t>()};
const auto &solution = pRoute->solutionVector.at(j);
if (solution.size() > 0) {
const auto &route = solution.at(0);
const auto &path = route.path;
const auto &info = route.info;
if (info.size() > 1) {
// Find index of first waypoint.
std::size_t idxFirst = 0;
const auto &infoFirst = info.at(1);
const auto &firstTransect = transectsENU[infoFirst.index];
if (firstTransect.size() > 0) {
const auto &firstWaypoint =
infoFirst.reversed ? firstTransect.back() : firstTransect.front();
double th = 0.01;
for (std::size_t i = 0; i < path.size(); ++i) {
auto dist = bg::distance(path[i], firstWaypoint);
if (dist < th) {
idxFirst = i;
break;
}
}
// Find index of last waypoint.
std::size_t idxLast = path.size() - 1;
const auto &infoLast = info.at(info.size() - 2);
const auto &lastTransect = transectsENU[infoLast.index];
if (lastTransect.size() > 0) {
const auto &lastWaypoint =
infoLast.reversed ? lastTransect.front() : lastTransect.back();
for (long i = path.size() - 1; i >= 0; --i) {
auto dist = bg::distance(path[i], lastWaypoint);
if (dist < th) {
idxLast = i;
break;
}
}
// Convert to geo coordinates.
auto &list = var.front();
for (std::size_t i = idxFirst; i <= idxLast; ++i) {
auto &vertex = path[i];
QGeoCoordinate c;
snake::fromENU(ori, vertex, c);
list.append(CoordInfo_t{c, CoordTypeInterior});
}
} else {
qCDebug(CircularSurveyLog)
<< "_setTransects(): lastTransect.size() == 0";
}
} else {
qCDebug(CircularSurveyLog)
<< "_setTransects(): firstTransect.size() == 0";
}
} else {
qCDebug(CircularSurveyLog)
<< "_setTransects(): transectsInfo.size() <= 1";
}
} else {
qCDebug(CircularSurveyLog) << "_setTransects(): solution.size() == 0";
}
if (var.size() > 0 && var.front().size() > 0) {
variantVector.push_back(std::move(var));
}
}
// Assign routes if no error occured.
if (variantVector.size() > 0) {
// Swap first route to _transects.
this->_variantVector.swap(variantVector);
// If the transects are getting rebuilt then any previously loaded
// mission items are now invalid.
if (_loadedMissionItemsParent) {
_loadedMissionItems.clear();
_loadedMissionItemsParent->deleteLater();
_loadedMissionItemsParent = nullptr;
}
// Add route variant names.
this->_variantNames.clear();
for (std::size_t i = 1; i <= std::size_t(this->_variantVector.size());
++i) {
this->_variantNames.append(QString::number(i));
}
emit variantNamesChanged();
disconnect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
this->_variant.setCookedValue(QVariant(0));
connect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
this->_changeVariantWorker();
this->_setState(STATE::SKIPP);
this->_rebuildTransects();
} else {
qCDebug(CircularSurveyLog)
<< "_setTransects(): failed, variantVector empty.";
this->_setState(STATE::IDLE);
}
}
Fact *CircularSurvey::variant() { return &_variant; }
bool CircularSurvey::calculating() const {
return this->_calculating(this->_state);
}
/*!
\class CircularSurveyComplexItem
\inmodule Wima
\brief The \c CircularSurveyComplexItem class provides a survey mission
item with circular transects around a point of interest.
CircularSurveyComplexItem class provides a survey mission item with
circular transects around a point of interest. Within the \c Wima module
it's used to scan a defined area with constant angle (circular transects)
to the base station (point of interest).
\sa WimaArea
*/
src/Wima/CircularSurvey.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QFutureWatcher>
#include <QVector>
#include <memory>
#include "SettingsFact.h"
#include "TransectStyleComplexItem.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "WimaPlanData.h"
class RoutingThread;
class RoutingData;
namespace routing {
class GeneratorBase;
}
class CircularSurvey : public TransectStyleComplexItem {
Q_OBJECT
using PtrGenerator = std::shared_ptr<routing::GeneratorBase>;
using PtrRoutingData = std::shared_ptr<RoutingData>;
using PtrWorker = std::unique_ptr<RoutingThread>;
using Transects = QList<QList<CoordInfo_t>>;
using Variant = Transects;
enum class STATE { IDLE, ROUTING, SKIPP, REVERSE, VARIANT_CHANGE };
public:
CircularSurvey(PlanMasterController *masterController, bool flyView,
const QString &kmlOrShpFile, QObject *parent);
~CircularSurvey();
Q_PROPERTY(Fact *variant READ variant CONSTANT)
Q_PROPERTY(
QStringList variantNames READ variantNames NOTIFY variantNamesChanged)
Q_PROPERTY(QStringList generatorNameList READ generatorNameList NOTIFY
generatorNameListChanged)
Q_PROPERTY(bool calculating READ calculating NOTIFY calculatingChanged)
Q_PROPERTY(
routing::GeneratorBase *generator READ generator NOTIFY generatorChanged)
Q_PROPERTY(int generatorIndex READ generatorIndex NOTIFY generatorChanged)
Q_INVOKABLE void reverse(void);
// Property setters
void setPlanData(const WimaPlanData &d);
// Property getters
const WimaPlanData &planData() const;
WimaPlanData &planData();
Fact *variant();
QStringList variantNames() const;
bool calculating() const;
// Overrides
virtual bool load(const QJsonObject &complexObject, int sequenceNumber,
QString &errorString) override final;
virtual QString mapVisualQML(void) const override final;
virtual void save(QJsonArray &planItems) override final;
virtual bool specifiesCoordinate(void) const override final;
virtual double timeBetweenShots(void) override final;
virtual QString commandDescription(void) const override final;
virtual QString commandName(void) const override final;
virtual QString abbreviation(void) const override final;
virtual ReadyForSaveState readyForSaveState(void) const override final;
virtual double additionalTimeDelay(void) const override final;
virtual QString patternName(void) const override;
// Generator
bool registerGenerator(const QString &name,
std::shared_ptr<routing::GeneratorBase> g);
bool unregisterGenerator(const QString &name);
bool unregisterGenerator(int index);
Q_INVOKABLE bool switchToGenerator(const QString &name);
Q_INVOKABLE bool switchToGenerator(int index);
QStringList generatorNameList();
routing::GeneratorBase *generator();
int generatorIndex();
static const char *settingsGroup;
static const char *variantName;
static const char *jsonComplexItemTypeValue;
static const QString name;
signals:
void calculatingChanged();
void variantNamesChanged();
void generatorNameListChanged();
void generatorChanged();
private slots:
// Overrides from TransectStyleComplexItem
virtual void _rebuildTransectsPhase1(void) override final;
virtual void _recalcCameraShots(void) override final;
// Worker functions.
void _setTransects(PtrRoutingData pRoute);
void _changeVariant();
bool _updateWorker();
void _changeVariantWorker();
void _reverseWorker();
private:
bool _switchToGenerator(const PtrGenerator &newG);
void _setState(STATE state);
bool _calculating(STATE state) const;
// State.
STATE _state;
// center of the circular lanes, e.g. base station
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _variant;
QStringList _variantNames;
// Area data
std::shared_ptr<WimaPlanData> _pAreaData;
// Generators
QList<PtrGenerator> _generatorList;
QStringList _generatorNameList;
PtrGenerator _pGenerator;
// Routing.
QVector<Variant> _variantVector;
PtrWorker _pWorker;
};
src/Wima/GenericSingelton.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "call_once.h"
#include <QScopedPointer>
#include <QtGlobal>
template <class T> class GenericSingelton {
private:
typedef T *(*CreateInstanceFunction)();
public:
static T *instance(CreateInstanceFunction create);
private:
static void init();
GenericSingelton();
~GenericSingelton();
Q_DISABLE_COPY(GenericSingelton)
static QBasicAtomicPointer<void> create;
static QBasicAtomicInt flag;
static QBasicAtomicPointer<void> tptr;
bool inited;
};
template <class T>
T *GenericSingelton<T>::instance(CreateInstanceFunction create) {
GenericSingelton::create.store(reinterpret_cast<void *>(create));
qCallOnce(init, flag);
return (T *)tptr.load();
}
template <class T> void GenericSingelton<T>::init() {
static GenericSingelton singleton;
if (singleton.inited) {
CreateInstanceFunction createFunction =
(CreateInstanceFunction)GenericSingelton::create.load();
tptr.store(createFunction());
}
}
template <class T> GenericSingelton<T>::GenericSingelton() { inited = true; }
template <class T> GenericSingelton<T>::~GenericSingelton() {
T *createdTptr = (T *)tptr.fetchAndStoreOrdered(nullptr);
if (createdTptr) {
delete createdTptr;
}
create.store(nullptr);
}
template <class T>
QBasicAtomicPointer<void>
GenericSingelton<T>::create = Q_BASIC_ATOMIC_INITIALIZER(nullptr);
template <class T>
QBasicAtomicInt GenericSingelton<T>::flag =
Q_BASIC_ATOMIC_INITIALIZER(CallOnce::CO_Request);
template <class T>
QBasicAtomicPointer<void>
GenericSingelton<T>::tptr = Q_BASIC_ATOMIC_INITIALIZER(nullptr);
src/Wima/Geometry/GenericCircle.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QObject>
#include <QPointF>
#include <cmath>
#include "boost/units/systems/angle/degrees.hpp"
#include "boost/units/systems/si.hpp"
template <int k, class Point> auto get(const 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;
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
auto x0 = get<0>(circ.origin());
auto y0 = get<1>(circ.origin());
auto r = circ.radius();
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;
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
auto x0 = get<0>(circ.origin());
auto y0 = get<1>(circ.origin());
auto r = circ.radius();
using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
while (n--) {
auto x = NumberType(x0 + r * std::cos(a));
auto y = NumberType(y0 + r * std::sin(a));
c.push_back(Point(x, y));
a += angleDisc;
}
}
src/Wima/Geometry/GenericPolygon.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QPolygonF>
#include <QPointF>
#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
template <class PointType = QPointF, template <class, class...> class ContainerType = QVector>
class GenericPolygon{ //
typedef ros_bridge::messages::geometry_msgs::polygon::GenericPolygon<PointType, ContainerType> Polygon;
public:
GenericPolygon(){}
GenericPolygon(const GenericPolygon &other) : _polygon(other._polygon){}
GenericPolygon& operator=(const GenericPolygon& other) {
this->_polygon = other._polygon;
return *this;
}
const Polygon &polygon() const {return _polygon;}
Polygon &polygon() {return _polygon;}
const ContainerType<PointType> &path() const {return _polygon.points();}
ContainerType<PointType> &path() {return _polygon.points();}
private:
Polygon _polygon;
};
src/Wima/Geometry/GenericPolygonArray.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "QmlObjectListModel.h"
#include <QVector>
#include <QString>
template <class PolygonType, template <class, class...> class ContainerType = QVector>
class GenericPolygonArray
{
public:
GenericPolygonArray() {}
GenericPolygonArray(const GenericPolygonArray &other) :
_polygons(other._polygons), _dirty(true)
{}
~GenericPolygonArray(){
_objs.clearAndDeleteContents();
}
class QmlObjectListModel *QmlObjectListModel(){
if (_dirty)
_updateObjects();
_dirty = false;
return &_objs;
}
const ContainerType<PolygonType> &polygons() const {
return _polygons;
}
ContainerType<PolygonType> &polygons(){
_dirty = true;
return _polygons;
}
GenericPolygonArray &operator =(const GenericPolygonArray &other){
this->_polygons = other._polygons;
this->_dirty = true;
return *this;
}
private:
void _updateObjects(void){
_objs.clearAndDeleteContents();
for (long i=0; i<_polygons.size(); ++i){
_objs.append(new PolygonType(_polygons[i]));
}
}
ContainerType<PolygonType> _polygons;
class QmlObjectListModel _objs;
bool _dirty;
};
src/Wima/Geometry/GeoPoint3D.cpp deleted 100644 → 0
View file @ 458ab210
#include "GeoPoint3D.h"
GeoPoint3D::GeoPoint3D(QObject *parent)
: QObject(parent), ROSGeoPoint() {}
GeoPoint3D::GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent)
: QObject(parent), ROSGeoPoint(latitude, longitude, altitude)
{}
GeoPoint3D::GeoPoint3D(const GeoPoint3D &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D::GeoPoint3D(const ROSGeoPoint &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
{
this->setLatitude(p.latitude());
this->setLongitude(p.longitude());
this->setAltitude(p.altitude());
return *this;
}
GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p)
{
this->setLatitude(p.latitude());
this->setLongitude(p.longitude());
this->setAltitude(p.altitude());
return *this;
}
src/Wima/Geometry/GeoPoint3D.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QGeoCoordinate>
#include <QObject>
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
typedef ros_bridge::messages::geographic_msgs::geo_point::GeoPoint ROSGeoPoint;
class GeoPoint3D : public QObject, public ROSGeoPoint
{
Q_OBJECT
public:
GeoPoint3D(QObject *parent = nullptr);
GeoPoint3D(double latitude,
double longitude,
double altitude,
QObject *parent = nullptr);
GeoPoint3D(const GeoPoint3D& p,
QObject *parent = nullptr);
GeoPoint3D(const ROSGeoPoint& p,
QObject *parent = nullptr);
GeoPoint3D(const QGeoCoordinate& p,
QObject *parent = nullptr);
GeoPoint3D &operator=(const GeoPoint3D&p);
GeoPoint3D &operator=(const QGeoCoordinate&p);
};
src/Wima/Geometry/PlanimetryCalculus.cc deleted 100644 → 0
View file @ 458ab210
#include "PlanimetryCalculus.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 (!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) {
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;
}
}
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;
}
}
/*!
* \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);
}
/*!
\fn IntersectType intersects(const Circle &circle1, const Circle &circle2)
Returns the intersection type of the two cirles \a circle1 and \a circle2.
\note Returns Error if circle.isNull() returns true;
\sa Circle
*/
bool intersects(const Circle &circle1, const Circle &circle2) {
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;
// }
// // determine line equations y = kx+d
// double dx1 = line1.dx();
// double dy1 = line1.dy();
// double k1;
// double d1 = 0;
// if (qFuzzyIsNull(dx1)) {
// k1 = std::numeric_limits<double>::infinity();
// } else {
// k1 = dy1/dx1;
// d1 = line1.y1()-k1*line1.x1();
// }
// double dx2 = line2.dx();
// double dy2 = line2.dy();
// double k2;
// double d2 = 0;
// if (qFuzzyIsNull(dx2)) {
// k2 = std::numeric_limits<double>::infinity();
// } else {
// k2 = dy2/dx2;
// d2 = line2.y1()-k2*line2.x1();
// }
// if ( !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;
// return false;
// }
// return false;
// }
// double x_intersect = -dd/dk;
// 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.
*/
src/Wima/Geometry/PlanimetryCalculus.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QLineF>
#include <QPointF>
#include <QPolygonF>
#include <QtMath>
#include "GenericCircle.h"
#include "PolygonCalculus.h"
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)); }
} // namespace PlanimetryCalculus
src/Wima/Geometry/PolygonCalculus.cc deleted 100644 → 0
View file @ 458ab210
#include "PolygonCalculus.h"
#include "PlanimetryCalculus.h"
#include "Wima/OptimisationTools.h"
#include <QVector3D>
#include <functional>
namespace PolygonCalculus {
namespace {
bool isReflexVertex(const QPolygonF& polygon, const QPointF *vertex) {
// Original Code from SurveyComplexItem::_VertexIsReflex()
auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
auto area = ( ((vertex->x() - vertexBefore->x())*(vertexAfter->y() - vertexBefore->y()))
-((vertexAfter->x() - vertexBefore->x())*(vertex->y() - vertexBefore->y())));
return area > 0;
}
} // end anonymous namespace
/*!
* \fn bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2)
* Returns true if the shortest path between the two coordinates is not fully inside the \a area.
*
* \sa QPointF, QPolygonF
*/
bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2)
{
if ( !polygon.isEmpty()) {
if ( !polygon.containsPoint(c1, Qt::FillRule::OddEvenFill)
|| !polygon.containsPoint(c2, Qt::FillRule::OddEvenFill))
return false;
QLineF line(c1, c2);
if (PlanimetryCalculus::intersectsFast(polygon, line))
return false;
return true;
} else {
return false;
}
}
/*!
* \fn int closestVertexIndex(const QPolygonF &polygon, const QPointF &coordinate)
* Returns the vertex index of \a polygon which has the least distance to \a coordinate.
*
* \sa QPointF, QPolygonF
*/
int closestVertexIndex(const QPolygonF &polygon, const QPointF &coordinate)
{
if (polygon.size() == 0) {
qWarning("Path is empty!");
return -1;
}else if (polygon.size() == 1) {
return 0;
}else {
int index = 0; // the index of the closest vertex
double min_dist = PlanimetryCalculus::distance(coordinate, polygon[index]);
for(int i = 1; i < polygon.size(); i++){
double dist = PlanimetryCalculus::distance(coordinate, polygon[i]);
if (dist < min_dist){
min_dist = dist;
index = i;
}
}
return index;
}
}
/*!auto distance
* \fn QPointF closestVertex(const QPolygonF &polygon, const QPointF &coordinate);
* Returns the vertex of \a polygon with the least distance to \a coordinate.
*
* \sa QPointF, QPolygonF
*/
QPointF closestVertex(const QPolygonF &polygon, const QPointF &coordinate)
{
int index = closestVertexIndex(polygon, coordinate);
if (index >=0 ) {
return polygon[index];
} else {
return QPointF();
}
}
/*!
* \fn int nextPolygonIndex(int pathsize, int index)
* Returns the index of the next vertex (of a polygon), which is \a index + 1 if \a index is smaller than \c {\a pathsize - 1},
* or 0 if \a index equals \c {\a pathsize - 1}, or -1 if the \a index is out of bounds.
* \note \a pathsize is usually obtained by invoking polygon.size()
*/
int nextVertexIndex(int pathsize, int index)
{
if (index >= 0 && index < pathsize-1) {
return index + 1;
} else if (index == pathsize-1) {
return 0;
} else {
qWarning("nextPolygonIndex(): Index out of bounds! index:count = %i:%i", index, pathsize);
return -1;
}
}
/*!
* \fn int previousPolygonIndex(int pathsize, int index)
* Returns the index of the previous vertex (of a polygon), which is \a index - 1 if \a index is larger 0,
* or \c {\a pathsize - 1} if \a index equals 0, or -1 if the \a index is out of bounds.
* \note pathsize is usually obtained by invoking polygon.size()
*/
int previousVertexIndex(int pathsize, int index)
{
if (index > 0 && index <pathsize) {
return index - 1;
} else if (index == 0) {
return pathsize-1;
} else {
qWarning("previousVertexIndex(): Index out of bounds! index:count = %i:%i", index, pathsize);
return -1;
}
}
/*!
* \fn JoinPolygonError joinPolygon(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon);
* Joins \a polygon1 and \a polygon2 such that a \l {Simple Polygon} is created.
* Stores the result inside \a joinedArea.
* Returns \c NotSimplePolygon1 if \a polygon1 isn't a Simple Polygon, \c NotSimplePolygon2 if \a polygon2 isn't a Simple Polygon, \c Disjoind if the polygons are disjoint,
* \c PathSizeLow if at least one polygon has a size samler than 3, or \c PolygonJoined on success.
* The algorithm will be able to join the areas, if either their edges intersect with each other,
* or one area is inside the other.
* The algorithm assumes that \a joinedPolygon is empty.
*/
JoinPolygonError join(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon)
{
using namespace PlanimetryCalculus;
if (polygon1.size() >= 3 && polygon2.size() >= 3) {
if ( !isSimplePolygon(polygon1) || !isSimplePolygon(polygon2)) {
return JoinPolygonError::NotSimplePolygon;
}
if (polygon1 == polygon2) {
joinedPolygon = polygon1;
return JoinPolygonError::PolygonJoined;
}
if ( !hasClockwiseWinding(polygon1)) {
reversePath(polygon1);
}
if ( !hasClockwiseWinding(polygon2)) {
reversePath(polygon2);
}
const QPolygonF *walkerPoly = &polygon1; // "walk" on this polygon towards higher indices
const QPolygonF *crossPoly = &polygon2; // check for crossings with this polygon while "walking"
// and swicht to this polygon on a intersection,
// continue to walk towards higher indices
// begin with the first index which is not inside crosspoly, if all Vertices are inside crosspoly return crosspoly
int startIndex = 0;
bool crossContainsWalker = true;
for (int i = 0; i < walkerPoly->size(); i++) {
if ( !contains(*crossPoly, walkerPoly->at(i)) ) {
crossContainsWalker = false;
startIndex = i;
break;
}
}
if ( crossContainsWalker == true) {
joinedPolygon.append(*crossPoly);
return JoinPolygonError::PolygonJoined;
}
QPointF currentVertex = walkerPoly->at(startIndex);
QPointF startVertex = currentVertex;
// possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly)
int nextVertexNumber = nextVertexIndex(walkerPoly->size(), startIndex);
QPointF protoNextVertex = walkerPoly->value(nextVertexNumber);
long counter = 0;
while (1) {
//qDebug("nextVertexNumber: %i", nextVertexNumber);
joinedPolygon.append(currentVertex);
QLineF walkerPolySegment;
walkerPolySegment.setP1(currentVertex);
walkerPolySegment.setP2(protoNextVertex);
QVector<QPair<int, int>> neighbourList;
QVector<QPointF> intersectionList;
//qDebug("IntersectionList.size() on init: %i", intersectionList.size());
PlanimetryCalculus::intersects(*crossPoly, walkerPolySegment, intersectionList, neighbourList);
//qDebug("IntersectionList.size(): %i", intersectionList.size());
if (intersectionList.size() > 0) {
int minDistIndex = -1;
double minDist = std::numeric_limits<double>::infinity();
for (int i = 0; i < intersectionList.size(); i++) {
double currentDist = PlanimetryCalculus::distance(currentVertex, intersectionList[i]);
if ( minDist > currentDist && !qFuzzyIsNull(distance(currentVertex, intersectionList[i])) ) {
minDist = currentDist;
minDistIndex = i;
}
}
if (minDistIndex != -1){
currentVertex = intersectionList.value(minDistIndex);
QPair<int, int> neighbours = neighbourList.value(minDistIndex);
protoNextVertex = crossPoly->value(neighbours.second);
nextVertexNumber = neighbours.second;
// switch walker and cross poly
const QPolygonF *temp = walkerPoly;
walkerPoly = crossPoly;
crossPoly = temp;
} else {
currentVertex = walkerPoly->value(nextVertexNumber);
nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber);
protoNextVertex = walkerPoly->value(nextVertexNumber);
}
} else {
currentVertex = walkerPoly->value(nextVertexNumber);
nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber);
protoNextVertex = walkerPoly->value(nextVertexNumber);
}
if (currentVertex == startVertex) {
if (polygon1.size() == joinedPolygon.size()) {
for (int i = 0; i < polygon1.size(); i++) {
if (polygon1[i] != joinedPolygon[i])
return PolygonJoined;
}
return Disjoint;
} else {
return PolygonJoined;
}
}
counter++;
if (counter > 1e5) {
qWarning("PolygonCalculus::join(): no successfull termination!");
return Error;
}
}
} else {
return PathSizeLow;
}
}
/*!
* \fn bool isSimplePolygon(const QPolygonF &polygon);
* Returns \c true if \a polygon is a \l {Simple Polygon}, \c false else.
* \note A polygon is a Simple Polygon iff it is not self intersecting.
*/
bool isSimplePolygon(const QPolygonF &polygon)
{
int i = 0;
if (polygon.size() > 3) {
// check if any edge of the area (formed by two adjacent vertices) intersects with any other edge of the area
while(i < polygon.size()-1) {
double cCIntersectCounter = 0; // corner corner intersection counter
QPointF refBeginCoordinate = polygon[i];
QPointF refEndCoordinate = polygon[nextVertexIndex(polygon.size(), i)];
QLineF refLine;
refLine.setP1(refBeginCoordinate);
refLine.setP2(refEndCoordinate);
int j = nextVertexIndex(polygon.size(), i);
while(j < polygon.size()) {
QPointF intersectionPt;
QLineF iteratorLine;
iteratorLine.setP1(polygon[j]);
iteratorLine.setP2(polygon[nextVertexIndex(polygon.size(), j)]);
PlanimetryCalculus::IntersectType intersectType;
PlanimetryCalculus::intersects(refLine, iteratorLine, intersectionPt, intersectType);
if ( intersectType == PlanimetryCalculus::CornerCornerIntersection) {
cCIntersectCounter++;
// max two corner corner intersections allowed, a specific coordinate can appear only once in a simple polygon
}
if ( cCIntersectCounter > 2
|| intersectType == PlanimetryCalculus::EdgeEdgeIntersection
|| intersectType == PlanimetryCalculus::EdgeCornerIntersection
|| intersectType == PlanimetryCalculus::LinesEqual
|| intersectType == PlanimetryCalculus::Error){
return false;
}
j++;
}
i++;
}
}
return true;
}
/*!
* \fn bool hasClockwiseWinding(const QPolygonF &polygon)
* Returns \c true if \a path has clockwiauto distancese winding, \c false else.
*/
bool hasClockwiseWinding(const QPolygonF &polygon)
{
if (polygon.size() <= 2) {
return false;
}
double sum = 0;
for (int i=0; i <polygon.size(); i++) {
QPointF coord1 = polygon[i];
QPointF coord2 = polygon[nextVertexIndex(polygon.size(), i)];
sum += (coord2.x() - coord1.x()) * (coord2.y() + coord1.y());
}
if (sum < 0.0)
return false;
return true;
}
/*!
* \fn void reversePath(QPolygonF &polygon)
* Reverses the path, i.e. changes the first Vertex with the last, the second with the befor last and so forth.
*/
void reversePath(QPolygonF &polygon)
{
QPolygonF pathReversed;
for (int i = 0; i < polygon.size(); i++) {
pathReversed.prepend(polygon[i]);
}
polygon.clear();
polygon.append(pathReversed);
}
/*!
* \fn void offsetPolygon(QPolygonF &polygon, double offset)
* Offsets a \a polygon by the given \a offset. The algorithm assumes that polygon is a \l {SimplePolygon}.
*/
void offsetPolygon(QPolygonF &polygon, double offset)
{
// Original code from QGCMapPolygon::offset()
QPolygonF newPolygon;
if (polygon.size() > 2) {
// Walk the edges, offsetting by theauto distance specified distance
QList<QLineF> rgOffsetEdges;
for (int i = 0; i < polygon.size(); i++) {
int nextIndex = nextVertexIndex(polygon.size(), i);
QLineF offsetEdge;
QLineF originalEdge(polygon[i], polygon[nextIndex]);
QLineF workerLine = originalEdge;
workerLine.setLength(offset);
workerLine.setAngle(workerLine.angle() - 90.0);
offsetEdge.setP1(workerLine.p2());
workerLine.setPoints(originalEdge.p2(), originalEdge.p1()); bool containsPath (const QPointF &c1, const QPointF &c2, QPolygonF polygon);
workerLine.setLength(offset);
workerLine.setAngle(workerLine.angle() + 90.0);
offsetEdge.setP2(workerLine.p2());
rgOffsetEdges << offsetEdge;
}
// Intersect the offset edges to generate new vertices
polygon.clear();
QPointF newVertex;
for (int i=0; i<rgOffsetEdges.count(); i++) {
int prevIndex = previousVertexIndex(rgOffsetEdges.size(), i);
if (rgOffsetEdges[prevIndex].intersect(rgOffsetEdges[i], &newVertex) == QLineF::NoIntersection) {
// FIXME: Better error handling?
qWarning("Intersection failed");
return;
}
polygon.append(newVertex);
}
}
}
void decomposeToConvex(const QPolygonF &polygon, QList<QPolygonF> &convexPolygons)
{
// Original Code SurveyComplexItem::_PolygonDecomposeConvex()
// this follows "Mark Keil's Algorithm" https://mpen.ca/406/keil
int decompSize = std::numeric_limits<int>::max();
if (polygon.size() < 3) return;
if (polygon.size() == 3) {
convexPolygons << polygon;
return;
}
QList<QPolygonF> decomposedPolygonsMin{};
for (const QPointF *vertex = polygon.begin(); vertex != polygon.end(); ++vertex)
{
// is vertex reflex?
bool vertexIsReflex = isReflexVertex(polygon, vertex);
if (!vertexIsReflex) continue;
for (const QPointF *vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther)
{
const QPointF *vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
const QPointF *vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
if (vertexOther == vertex) continue;
if (vertexAfter == vertexOther) continue;
if (vertexBefore == vertexOther) continue;
bool canSee = containsPath(polygon, *vertex, *vertexOther);
if (!canSee) continue;
QPolygonF polyLeft;
const QPointF *v = vertex;
bool polyLeftContainsReflex = false;
while ( v != vertexOther) {
if (v != vertex && isReflexVertex(polygon, v)) {
polyLeftContainsReflex = true;
}
polyLeft << *v;
++v;
if (v == polygon.end()) v = polygon.begin();
}
polyLeft << *vertexOther;
bool polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3);
QPolygonF polyRight;
v = vertexOther;
bool polyRightContainsReflex = false;
while ( v != vertex) {
if (v != vertex && isReflexVertex(polygon, v)) {
polyRightContainsReflex = true;
}
polyRight << *v;
++v;
if (v == polygon.end()) v = polygon.begin();
}
polyRight << *vertex;
auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3);
if (!polyLeftValid || ! polyRightValid) {
// decompSize = std::numeric_limits<int>::max();
continue;
}
// recursion
QList<QPolygonF> polyLeftDecomposed{};
decomposeToConvex(polyLeft, polyLeftDecomposed);
QList<QPolygonF> polyRightDecomposed{};
decomposeToConvex(polyRight, polyRightDecomposed);
// compositon
int subSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
if ( (polyLeftContainsReflex && polyLeftDecomposed.size() == 1)
|| (polyRightContainsReflex && polyRightDecomposed.size() == 1))
{
// don't accept polygons that contian reflex vertices and were not split
subSize = std::numeric_limits<int>::max();
}
if (subSize < decompSize) {
decompSize = subSize;
decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
}
}
}
// assemble output
if (decomposedPolygonsMin.size() > 0) {
convexPolygons << decomposedPolygonsMin;
} else {
convexPolygons << polygon;
}
return;
}
bool shortestPath(const QPolygonF &polygon,const QPointF &startVertex, const QPointF &endVertex, QVector<QPointF> &shortestPath)
{
using namespace PlanimetryCalculus;
QPolygonF bigPolygon(polygon);
offsetPolygon(bigPolygon, 0.5); // solves numerical errors
if ( bigPolygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill)
&& bigPolygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) {
QVector<QPointF> elementList;
elementList.append(startVertex);
elementList.append(endVertex);
for (auto vertex : polygon) elementList.append(vertex);
const int listSize = elementList.size();
std::function<double(const int, const int)> distanceDij = [bigPolygon, elementList](const int ind1, const int ind2) -> double {
QPointF p1 = elementList[ind1];
QPointF p2 = elementList[ind2];
double dist = std::numeric_limits<double>::infinity();
if (containsPathFast(bigPolygon, p1, p2)){ // containsPathFast can be used since all points are inside bigPolygon
double dx = p1.x()-p2.x();
double dy = p1.y()-p2.y();
dist = sqrt((dx*dx)+(dy*dy));
}
return dist;
};
QVector<int> shortestPathIndex;
bool retVal = OptimisationTools::dijkstraAlgorithm(listSize, 0, 1, shortestPathIndex, distanceDij);
for (auto index : shortestPathIndex) shortestPath.append(elementList[index]);
return retVal;
} else {
return false;
}
}
QVector3DList toQVector3DList(const QPolygonF &polygon)
{
QVector3DList list;
for ( auto vertex : polygon )
list.append(QVector3D(vertex));
return list;
}
QPolygonF toQPolygonF(const QPointFList &listF)
{
QPolygonF polygon;
for ( auto vertex : listF )
polygon.append(vertex);
return polygon;
}
QPointFList toQPointFList(const QPolygonF &polygon)
{
QPointFList listF;
for ( auto vertex : polygon )
listF.append(vertex);
return listF;
}
void reversePath(QPointFList &path)
{
QPointFList pathReversed;
for ( auto element : path) {
pathReversed.prepend(element);
}
path = pathReversed;
}
void reversePath(QPointFVector &path)
{
QPointFVector pathReversed;
for ( auto element : path) {
pathReversed.prepend(element);
}
path = pathReversed;
}
bool containsPathFast(QPolygonF polygon, const QPointF &c1, const QPointF &c2)
{
if ( !polygon.isEmpty()) {
QLineF line(c1, c2);
if (PlanimetryCalculus::intersectsFast(polygon, line))
return false;
return true;
} else {
return false;
}
}
} // end PolygonCalculus namespace
src/Wima/Geometry/PolygonCalculus.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QPointF>
#include <QPolygonF>
#include <QVector3D>
namespace PolygonCalculus {
enum JoinPolygonError { NotSimplePolygon, PolygonJoined, Disjoint, PathSizeLow, Error};
typedef QList<QVector3D> QVector3DList;
typedef QList<QPointF> QPointFList;
typedef QVector<QPointF> QPointFVector;
int closestVertexIndex (const QPolygonF &polygon, const QPointF &coordinate);
QPointF closestVertex (const QPolygonF &polygon, const QPointF &coordinate);
int nextVertexIndex (int pathsize, int index);
int previousVertexIndex (int pathsize, int index);
JoinPolygonError join (QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon);
bool isSimplePolygon (const QPolygonF &polygon);
bool hasClockwiseWinding (const QPolygonF &path);
void reversePath (QPolygonF &path);
void reversePath (QPointFList &path);
void reversePath (QPointFVector &path);
void offsetPolygon (QPolygonF &polygon, double offset);
// returns true if the line c1-c2 is fully inside the polygon
bool containsPath (QPolygonF polygon, const QPointF &c1, const QPointF &c2);
// same as containsPath(), but works only if c1 and c2 are inside the polygon!
bool containsPathFast (QPolygonF polygon, const QPointF &c1, const QPointF &c2);
void decomposeToConvex (const QPolygonF &polygon, QList<QPolygonF> &convexPolygons);
bool shortestPath (const QPolygonF &polygon, const QPointF &startVertex, const QPointF &endVertex, QVector<QPointF> &shortestPath);
QPolygonF toQPolygonF(const QVector3DList &polygon);
QPolygonF toQPolygonF(const QPointFList &polygon);
QLineF toQLineF(const QVector3DList &line);
QPointFList toQPointFList(const QVector3DList &list);
QPointFList toQPointFList(const QPolygonF &list);
QVector3DList toQVector3DList(const QPointFList &listF);
QVector3DList toQVector3DList(const QPolygonF &listF);
}
src/Wima/Geometry/TestPolygonCalculus.cpp deleted 100644 → 0
View file @ 458ab210
#include "TestPolygonCalculus.h"
#include "PolygonCalculus.h"
#include <QPolygonF>
#include <QPointF>
namespace TestPolygonCalculus {
void test()
{
using namespace PolygonCalculus;
QPolygonF polygon;
polygon << QPointF(0, 0) << QPointF(1, 0) << QPointF(1, 1) << QPointF(0, 1);
bool retVal = containsPath(polygon, QPointF(0.1, 0.1), QPointF(0.2, 0.1));
qDebug("true %i", retVal);
retVal = containsPath(polygon, QPointF(0.2, 0.1), QPointF(0.2, 0.1));
qDebug("true %i", retVal);
retVal = containsPath(polygon, QPointF(1, 0.1), QPointF(0.2, 0.1));
qDebug("true %i", retVal);
retVal = containsPath(polygon, QPointF(0.1, 0.1), QPointF(1, 0.1));
qDebug("true %i", retVal);
retVal = containsPath(polygon, QPointF(2, 0.1), QPointF(0.2, 0.1));
qDebug("false %i", retVal);
retVal = containsPath(polygon, QPointF(-2, 0.1), QPointF(0.2, 0.1));
qDebug("false %i", retVal);
polygon << QPointF(0.5, 0.6) << QPointF(0.5, 0.4);
retVal = containsPath(polygon, QPointF(0.2, 0.1), QPointF(0.2, 0.9));
qDebug("false %i", retVal);
retVal = containsPath(polygon, QPointF(0.1, 0.05), QPointF(0.51, 0.6));
qDebug("false %i", retVal);
}
}
src/Wima/Geometry/TestPolygonCalculus.h deleted 100644 → 0
View file @ 458ab210
#ifndef TESTPOLYGONCALCULUS_H
#define TESTPOLYGONCALCULUS_H
namespace TestPolygonCalculus {
void test();
}
#endif // TESTPOLYGONCALCULUS_H
src/Wima/Geometry/WimaArea.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaArea.h"
/*!
* \variable WimaArea::epsilonMeter
* \brief The accuracy used for distance calculations (unit: m).
*/
const double WimaArea::epsilonMeter = 1e-5;
/*!
* \variable WimaArea::maxAltitudeName
* \brief A string containing the name of the \c _maxAltitude member. Among
* other used for storing.
*/
const char *WimaArea::maxAltitudeName = "maxAltitude";
/*!
* \variable WimaArea::wimaAreaName
* \brief A string containing the name of this \c WimaArea member. Among other
* used for storing.
*/
const char *WimaArea::wimaAreaName = "WimaArea";
/*!
* \variable WimaArea::areaTypeName
* \brief A string containing \c {"AreaType"}. Among other used for stroing.
*/
const char *WimaArea::areaTypeName = "AreaType";
const char *WimaArea::borderPolygonOffsetName = "BorderPolygonOffset";
const char *WimaArea::showBorderPolygonName = "ShowBorderPolygon";
const char *WimaArea::settingsGroup = "MeasurementArea";
// Constructors
WimaArea::WimaArea(QObject *parent)
: QGCMapPolygon(parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaArea.SettingsGroup.json"),
this /* QObject parent */)),
_borderPolygonOffset(SettingsFact(settingsGroup,
_metaDataMap[borderPolygonOffsetName],
this /* QObject parent */)),
_showBorderPolygon(SettingsFact(settingsGroup,
_metaDataMap[showBorderPolygonName],
this /* QObject parent */)),
_borderPolygon(QGCMapPolygon(this)), _wimaAreaInteractive(false) {
init();
_maxAltitude = 30;
}
WimaArea::WimaArea(const WimaArea &other, QObject *parent)
: QGCMapPolygon(parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaArea.SettingsGroup.json"),
this /* QObject parent */)),
_borderPolygonOffset(SettingsFact(settingsGroup,
_metaDataMap[borderPolygonOffsetName],
this /* QObject parent */)),
_showBorderPolygon(SettingsFact(settingsGroup,
_metaDataMap[showBorderPolygonName],
this /* QObject parent */)),
_borderPolygon(QGCMapPolygon(this)), _wimaAreaInteractive(false) {
init();
*this = other;
}
/*!
*\fn WimaArea &WimaArea::operator=(const WimaArea &other)
*
* Assigns \a other to this \c WimaArea and returns a reference to this \c
*WimaArea.
*
* Copies only path and maximum altitude.
*/
WimaArea &WimaArea::operator=(const WimaArea &other) {
QGCMapPolygon::operator=(other);
this->setMaxAltitude(other._maxAltitude);
this->setPath(other.path());
return *this;
}
void WimaArea::setWimaAreaInteractive(bool interactive) {
if (WimaArea::_wimaAreaInteractive != interactive) {
WimaArea::_wimaAreaInteractive = interactive;
emit WimaArea::wimaAreaInteractiveChanged();
}
}
/*!
\fn void WimaArea::setMaxAltitude(double altitude)
Sets the \c _maxAltitude member to \a altitude and emits the signal \c
maxAltitudeChanged() if \c _maxAltitude is not equal to altitude.
*/
void WimaArea::setMaxAltitude(double altitude) {
if (altitude > 0 && qFuzzyCompare(altitude, _maxAltitude)) {
_maxAltitude = altitude;
emit maxAltitudeChanged();
}
}
void WimaArea::setShowBorderPolygon(bool showBorderPolygon) {
_showBorderPolygon.setRawValue(showBorderPolygon);
}
void WimaArea::setBorderPolygonOffset(double offset) {
if (!qFuzzyCompare(_borderPolygonOffset.rawValue().toDouble(), offset)) {
_borderPolygonOffset.setRawValue(offset);
emit borderPolygonOffsetChanged();
}
}
void WimaArea::recalcPolygons() {
if (_showBorderPolygon.rawValue().toBool() == true) {
if (_borderPolygon.count() >= 3) {
//_borderPolygon.verifyClockwiseWinding(); // causes seg. fault
this->setPath(_borderPolygon.coordinateList());
this->offset(-_borderPolygonOffset.rawValue().toDouble());
}
} else {
if (this->count() >= 3) {
// this->verifyClockwiseWinding(); // causes seg. fault
_borderPolygon.setPath(this->coordinateList());
_borderPolygon.offset(_borderPolygonOffset.rawValue().toDouble());
}
emit borderPolygonChanged();
}
}
void WimaArea::updatePolygonConnections(QVariant showBorderPolygon) {
if (showBorderPolygon.toBool() == true) {
connect(&_borderPolygon, &QGCMapPolygon::pathChanged, this,
&WimaArea::recalcPolygons);
disconnect(this, &QGCMapPolygon::pathChanged, this,
&WimaArea::recalcPolygons);
} else {
disconnect(&_borderPolygon, &QGCMapPolygon::pathChanged, this,
&WimaArea::recalcPolygons);
connect(this, &QGCMapPolygon::pathChanged, this, &WimaArea::recalcPolygons);
}
}
void WimaArea::recalcInteractivity() {
if (_wimaAreaInteractive == false) {
QGCMapPolygon::setInteractive(false);
_borderPolygon.setInteractive(false);
} else {
if (_showBorderPolygon.rawValue().toBool() == true) {
_borderPolygon.setInteractive(true);
QGCMapPolygon::setInteractive(false);
} else {
_borderPolygon.setInteractive(false);
QGCMapPolygon::setInteractive(true);
}
}
}
/*!
* \fn int WimaArea::getClosestVertexIndex(const QGeoCoordinate &coordinate)
* const Returns the index of the vertex (element of the polygon path) which has
* the least distance to \a coordinate.
*
* \sa QGeoCoordinate
*/
int WimaArea::getClosestVertexIndex(const QGeoCoordinate &coordinate) const {
if (this->count() == 0) {
qWarning("Polygon count == 0!");
return -1;
} else if (this->count() == 1) {
return 0;
} else {
int index = 0;
double min_dist = coordinate.distanceTo(this->vertexCoordinate(index));
for (int i = 1; i < this->count(); i++) {
double dist = coordinate.distanceTo(this->vertexCoordinate(i));
if (dist < min_dist) {
min_dist = dist;
index = i;
}
}
return index;
}
}
/*!
* \fn QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate&
* coordinate) const Returns the vertex of the polygon path with the least
* distance to \a coordinate.
*
* \sa QGeoCoordinate
*/
QGeoCoordinate
WimaArea::getClosestVertex(const QGeoCoordinate &coordinate) const {
return this->vertexCoordinate(getClosestVertexIndex(coordinate));
}
/*!
* \fn QGCMapPolygon WimaArea::toQGCPolygon(const WimaArea &area)
* Converts the \c WimaArea \a area to \c QGCMapPolygon by copying the path
* only.
*/
QGCMapPolygon WimaArea::toQGCPolygon(const WimaArea &area) {
QGCMapPolygon qgcPoly;
qgcPoly.setPath(area.path());
return QGCMapPolygon(qgcPoly);
}
/*!
* \fn QGCMapPolygon WimaArea::toQGCPolygon() const
* Converts the calling \c WimaArea to \c QGCMapPolygon by copying the path
* only.
*/
QGCMapPolygon WimaArea::toQGCPolygon() const { return toQGCPolygon(*this); }
/*!
* \fn bool WimaArea::join(WimaArea &area1, WimaArea &area2, WimaArea
* &joinedArea, QString &errorString) Joins the areas \a area1 and \a area2 such
* that a \l {Simple Polygon} is created. Stores the result inside \a
* joinedArea. Stores error messages in \a errorString. Returns \c true if the
* algorithm was able to join the areas; false else. The algorithm will be able
* to join the areas, if either their edges intersect with each other, or one
* area contains the other.
*/
bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
WimaArea &joinedArea, QString &errorString) {
using namespace GeoUtilities;
using namespace PolygonCalculus;
Q_UNUSED(errorString);
QList<QGeoCoordinate> GeoPolygon1 = area1.coordinateList();
QList<QGeoCoordinate> GeoPolygon2 = area2.coordinateList();
// qWarning("befor joining");
// qWarning() << GeoPolygon1;
// qWarning() << GeoPolygon2;
QGeoCoordinate origin = GeoPolygon1[0];
// QGeoCoordinate tset = GeoPolygon1[2];
// qWarning() << tset;qWarning() << toGeo(toCartesian2D(tset, origin),
// origin);
QPolygonF polygon1;
toCartesianList(GeoPolygon1, origin, polygon1);
QPolygonF polygon2;
toCartesianList(GeoPolygon2, origin, polygon2);
// qWarning("after 1 transform");
// qWarning() << polygon1;
// qWarning() << polygon2;
QPolygonF joinedPolygon;
JoinPolygonError retValue =
PolygonCalculus::join(polygon1, polygon2, joinedPolygon);
// qWarning("after joining");
// qWarning() << joinedPolygon;
if (retValue == JoinPolygonError::Disjoint) {
qWarning("Polygons are disjoint.");
} else if (retValue == JoinPolygonError::NotSimplePolygon) {
qWarning("Not a simple polygon.");
} else if (retValue == JoinPolygonError::PathSizeLow) {
qWarning("Polygon vertex count is low.");
} else {
QList<QGeoCoordinate> path;
toGeoList(joinedPolygon, origin, path);
// qWarning("after transform");
// qWarning() << path;
joinedArea.setPath(path);
return true;
}
return false;
}
/*!
* \fn bool WimaArea::join(WimaArea &area1, WimaArea &area2, WimaArea
* &joinedArea) Joins the areas \a area1 and \a area2 such that a \l {Simple
* Polygon} is created. Stores the result inside \a joinedArea. Returns \c true
* if the algorithm was able to join the areas; false else. The algorithm will
* be able to join the areas, if either their edges intersect with each other,
* or one area contains the other.
*/
bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
WimaArea &joinedArea) {
QString dummy;
return join(area1, area2, joinedArea, dummy);
}
/*!
* \fn bool WimaArea::join(WimaArea &area)
* Joins the calling \c WimaArea and the \a area such that a \l {Simple Polygon}
* is created. Overwrites the calling \c WimaArea with the result, if the
* algorithm was successful. Returns \c true if the algorithm was able to join
* the areas; false else. The algorithm will be able to join the areas, if
* either their edges intersect with each other, or one area contains the other.
*/
bool WimaArea::join(WimaArea &area) {
WimaArea joinedArea;
if (join(*this, area, joinedArea)) {
// qWarning("WimaArea::join(WimaArea &area)");
// qWarning() << joinedArea.coordinateList();
this->setPath(joinedArea.path());
return true;
} else {
return false;
}
}
/*!
* \fn bool WimaArea::join(WimaArea &area, QString &errorString)
* Joins the calling \c WimaArea and the \a area such that a \l {Simple Polygon}
* is created. Overwrites the calling \c WimaArea with the result, if the
* algorithm was successful.
*
* Returns \c true if the algorithm was able to join the areas; false else.
* Stores error messages in \a errorString.
*
* The algorithm will be able to join the areas, if either their edges intersect
* with each other, or one area contains the other.
*/
bool WimaArea::join(WimaArea &area, QString &errorString) {
WimaArea joinedArea;
if (join(*this, area, joinedArea, errorString)) {
this->setPath(joinedArea.path());
return true;
} else {
return false;
}
}
/*!
* \fn int WimaArea::nextVertexIndex(int index) const
* Returns the index of the next vertex (of the areas path), which is \a index +
* 1 if \a index is smaller than \c {area.count() - 1}, or 0 if \a index equals
* \c {area.count() - 1}, or -1 if the \a index is out of bounds. \note The
* function \c {area.count()} (derived from \c QGCMapPolygon) returns the number
* of vertices defining the area.
*/
int WimaArea::nextVertexIndex(int index) const {
if (index >= 0 && index < count() - 1) {
return index + 1;
} else if (index == count() - 1) {
return 0;
} else {
qWarning(
"WimaArea::nextVertexIndex(): Index out of bounds! index:count = %i:%i",
index, count());
return -1;
}
}
/*!
* \fn int WimaArea::previousVertexIndex(int index) const
* Returns the index of the previous vertex (of the areas path), which is \a
* index - 1 if \a index is larger 0, or \c {area.count() - 1} if \a index
* equals 0, or -1 if the \a index is out of bounds. \note The function \c
* {area.count()} (derived from \c QGCMapPolygon) returns the number of vertices
* defining the area.
*/
int WimaArea::previousVertexIndex(int index) const {
if (index > 0 && index < count()) {
return index - 1;
} else if (index == 0) {
return count() - 1;
} else {
qWarning("WimaArea::previousVertexIndex(): Index out of bounds! "
"index:count = %i:%i",
index, count());
return -1;
}
}
/*!
* \fn bool WimaArea::isSelfIntersecting()
* Returns \c true if the calling area is self intersecting, \c false else.
* \note If the calling area is self intersecting, it's not a \l {Simple
* Polygon}.
*/
bool WimaArea::isSimplePolygon() const {
using namespace PolygonCalculus;
using namespace GeoUtilities;
if (this->count() > 2) {
QPolygonF polygon;
toCartesianList(this->coordinateList(), this->vertexCoordinate(0), polygon);
return PolygonCalculus::isSimplePolygon(polygon);
} else
return false;
}
bool WimaArea::containsCoordinate(const QGeoCoordinate &coordinate) const {
using namespace PlanimetryCalculus;
using namespace PolygonCalculus;
using namespace GeoUtilities;
if (this->count() > 2) {
QPolygonF polygon;
toCartesianList(this->coordinateList(), coordinate, polygon);
return PlanimetryCalculus::contains(polygon, QPointF(0, 0));
} else
return false;
}
/*!
* \fn void WimaArea::saveToJson(QJsonObject &json)
* Saves the calling area to \c QJsonObject object and stores it inside \a json.
*
* \sa QJsonObject
*/
void WimaArea::saveToJson(QJsonObject &json) {
this->QGCMapPolygon::saveToJson(json);
json[maxAltitudeName] = _maxAltitude;
json[borderPolygonOffsetName] = _borderPolygonOffset.rawValue().toDouble();
json[showBorderPolygonName] = _showBorderPolygon.rawValue().toDouble();
json[areaTypeName] = wimaAreaName;
}
/*!
* \fn bool WimaArea::loadFromJson(const QJsonObject &json, QString&
* errorString) Loads data from \a json and stores it inside the calling area.
* Returns \c true if loading was successful, \c false else.
* Stores error messages inside \a errorString.
*
* \sa QJsonObject
*/
bool WimaArea::loadFromJson(const QJsonObject &json, QString &errorString) {
if (this->QGCMapPolygon::loadFromJson(json, false /*no poly required*/,
errorString)) {
if (json.contains(maxAltitudeName) && json[maxAltitudeName].isDouble()) {
_maxAltitude = json[maxAltitudeName].toDouble();
} else {
errorString.append(tr("Could not load Maximum Altitude value!\n"));
return false;
}
if (json.contains(borderPolygonOffsetName) &&
json[borderPolygonOffsetName].isDouble()) {
_borderPolygonOffset.setRawValue(
json[borderPolygonOffsetName].toDouble());
} else {
errorString.append(tr("Could not load border polygon offset value!\n"));
return false;
}
if (json.contains(showBorderPolygonName) &&
json[showBorderPolygonName].isDouble()) {
_showBorderPolygon.setRawValue(json[showBorderPolygonName].toBool());
} else {
errorString.append(tr("Could not load border polygon offset value!\n"));
return false;
}
} else {
qWarning() << errorString;
return false;
}
return true;
}
/*!
* \fn void WimaArea::init()
* Funtion to be called during construction.
*/
void WimaArea::init() {
this->setObjectName(wimaAreaName);
if (_showBorderPolygon.rawValue().toBool() == true) {
connect(&_borderPolygon, &QGCMapPolygon::pathChanged, this,
&WimaArea::recalcPolygons);
} else {
connect(this, &QGCMapPolygon::pathChanged, this, &WimaArea::recalcPolygons);
}
connect(&_borderPolygonOffset, &SettingsFact::rawValueChanged, this,
&WimaArea::recalcPolygons);
connect(&_showBorderPolygon, &SettingsFact::rawValueChanged, this,
&WimaArea::updatePolygonConnections);
connect(&_showBorderPolygon, &SettingsFact::rawValueChanged, this,
&WimaArea::recalcInteractivity);
connect(this, &WimaArea::wimaAreaInteractiveChanged, this,
&WimaArea::recalcInteractivity);
}
// QDoc Documentation
/*!
\group WimaAreaGroup
\title Group of WimaAreas
Every \c WimaArea of the equally named group uses a \l {Simple Polygon}
derived from \c {QGCMapPolygon} to define areas inside which certain taskts
are performed.
*/
/*!
\class WimaArea
\inmodule Wima
\ingroup WimaArea
\brief The \c WimaArea class provides the a base class for
all areas used within the Wima extension.
\c WimaArea uses a \l {Simple Polygon} derived from \c {QGCMapPolygon}
to define areas inside which certain taskts are performed. The polygon
(often refered to as the path) can be displayed visually on a map.
*/
/*!
\variable WimaArea::_maxAltitude
\brief The maximum altitude vehicles are allowed to fly inside this area.
*/
/*!
\property WimaArea::maxAltitude
\brief The maximum altitude at which vehicles are allowed to fly.
*/
/*!
\property WimaArea::mapVisualQML
\brief A string containing the name of the QML file used to displays this area
on a map.
*/
/*!
\property WimaArea::editorQML
\brief A string containing the name of the QML file allowing to edit the
area's properties.
*/
/*!
\externalpage https://en.wikipedia.org/wiki/Simple_polygon
\title Simple Polygon
*/
/*!
\externalpage https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
\title Dijkstra Algorithm
*/
src/Wima/Geometry/WimaArea.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "QGCGeo.h"
#include "QGCMapPolygon.h"
#include "QGCMapPolyline.h"
#include "Vehicle.h"
#include "qobject.h"
#include <QLineF>
#include <QPair>
#include <QPointF>
#include "GeoUtilities.h"
#include "PlanimetryCalculus.h"
#include "PolygonCalculus.h"
class WimaArea : public QGCMapPolygon // abstract base class for all WimaAreas
{
Q_OBJECT
public:
WimaArea(QObject *parent = nullptr);
WimaArea(const WimaArea &other, QObject *parent = nullptr);
WimaArea &operator=(const WimaArea &other);
Q_PROPERTY(double maxAltitude READ maxAltitude WRITE setMaxAltitude NOTIFY
maxAltitudeChanged)
Q_PROPERTY(QString mapVisualQML READ mapVisualQML CONSTANT)
Q_PROPERTY(QString editorQML READ editorQML CONSTANT)
Q_PROPERTY(Fact *borderPolygonOffset READ borderPolygonOffsetFact CONSTANT)
Q_PROPERTY(QGCMapPolygon *borderPolygon READ borderPolygon NOTIFY
borderPolygonChanged)
Q_PROPERTY(Fact *showBorderPolygon READ showBorderPolygon CONSTANT)
Q_PROPERTY(bool wimaAreaInteractive READ wimaAreaInteractive WRITE
setWimaAreaInteractive NOTIFY wimaAreaInteractiveChanged)
// Property accessors
double maxAltitude(void) const { return _maxAltitude; }
Fact *borderPolygonOffsetFact(void) { return &_borderPolygonOffset; }
Fact *showBorderPolygon(void) { return &_showBorderPolygon; }
double borderPolygonOffset(void) const {
return _borderPolygonOffset.rawValue().toDouble();
}
QGCMapPolygon *borderPolygon(void) { return &_borderPolygon; }
bool wimaAreaInteractive(void) const { return _wimaAreaInteractive; }
void setWimaAreaInteractive(bool interactive);
// overrides from WimaArea
virtual QString mapVisualQML(void) const { return ""; }
virtual QString editorQML(void) const { return ""; }
// Member Methodes
int getClosestVertexIndex(const QGeoCoordinate &coordinate) const;
QGeoCoordinate getClosestVertex(const QGeoCoordinate &coordinate) const;
QGCMapPolygon toQGCPolygon() const;
bool join(WimaArea &area);
bool join(WimaArea &area, QString &errorString);
int nextVertexIndex(int index) const;
int previousVertexIndex(int index) const;
bool isSimplePolygon() const;
bool containsCoordinate(const QGeoCoordinate &coordinate) const;
void saveToJson(QJsonObject &jsonObject);
bool loadFromJson(const QJsonObject &jsonObject, QString &errorString);
// static Methodes
static QGCMapPolygon toQGCPolygon(const WimaArea &area);
static bool join(const WimaArea &area1, const WimaArea &area2,
WimaArea &joinedArea, QString &errorString);
static bool join(const WimaArea &area1, const WimaArea &area2,
WimaArea &joinedArea);
// Friends
friend void print(const WimaArea &area, QString &outputString);
friend void print(const WimaArea &area);
// static Members
// Accurracy used to compute isDisjunct
static const double epsilonMeter;
static const char *maxAltitudeName;
static const char *wimaAreaName;
static const char *areaTypeName;
static const char *borderPolygonOffsetName;
static const char *showBorderPolygonName;
static const char *settingsGroup;
signals:
void maxAltitudeChanged(void);
void borderPolygonChanged(void);
void borderPolygonOffsetChanged(void);
void wimaAreaInteractiveChanged(void);
public slots:
void setMaxAltitude(double altitude);
void setShowBorderPolygon(bool showBorderPolygon);
void setBorderPolygonOffset(double offset);
private slots:
void recalcPolygons(void);
void updatePolygonConnections(QVariant value);
void recalcInteractivity(void);
private:
void init();
double _maxAltitude;
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _borderPolygonOffset;
SettingsFact _showBorderPolygon;
QGCMapPolygon _borderPolygon;
bool _wimaAreaInteractive;
};
src/Wima/Geometry/WimaAreaData.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaAreaData.h"
WimaAreaData::WimaAreaData(QObject *parent) : QObject(parent) {}
WimaAreaData::WimaAreaData(const WimaAreaData &other, QObject *parent)
: QObject(parent), _path(other._path), _list(other._list),
_center(other._center) {}
WimaAreaData::WimaAreaData(const WimaArea &otherData, QObject *parent)
: QObject(parent), _center(otherData.center()) {
_setPathImpl(otherData.path());
}
bool WimaAreaData::operator==(const WimaAreaData &data) const {
return this->_path == data._path && this->_center == data._center;
}
bool WimaAreaData::operator!=(const WimaAreaData &data) const {
return !this->operator==(data);
}
QVariantList WimaAreaData::path() const { return _path; }
QGeoCoordinate WimaAreaData::center() const { return _center; }
const QList<QGeoCoordinate> &WimaAreaData::coordinateList() const {
return _list;
}
bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const {
using namespace PlanimetryCalculus;
using namespace PolygonCalculus;
using namespace GeoUtilities;
if (this->coordinateList().size() > 2) {
QPolygonF polygon;
toCartesianList(this->coordinateList(), coordinate, polygon);
return PlanimetryCalculus::contains(polygon, QPointF(0, 0));
} else
return false;
}
void WimaAreaData::append(const QGeoCoordinate &c) {
_list.append(c);
_path.push_back(QVariant::fromValue(c));
emit pathChanged();
}
void WimaAreaData::push_back(const QGeoCoordinate &c) { append(c); }
void WimaAreaData::clear() {
if (_list.size() > 0 || _path.size() > 0) {
_list.clear();
_path.clear();
emit pathChanged();
}
}
void WimaAreaData::setPath(const QVariantList &coordinateList) {
if (_path != coordinateList) {
_setPathImpl(coordinateList);
emit pathChanged();
}
}
void WimaAreaData::setCenter(const QGeoCoordinate &center) {
if (_center != center) {
_center = center;
emit centerChanged();
}
}
WimaAreaData &WimaAreaData::operator=(const WimaAreaData &otherData) {
setPath(otherData._list);
setCenter(otherData._center);
return *this;
}
WimaAreaData &WimaAreaData::operator=(const WimaArea &otherData) {
setPath(otherData.path());
setCenter(otherData.center());
return *this;
}
void WimaAreaData::_setPathImpl(const QList<QGeoCoordinate> &coordinateList) {
_list = coordinateList;
_path.clear();
// copy all coordinates to _path
for (const auto &vertex : coordinateList) {
_path.append(QVariant::fromValue(vertex));
}
}
void WimaAreaData::_setPathImpl(const QVariantList &coordinateList) {
_path = coordinateList;
_list.clear();
for (const auto &variant : coordinateList) {
_list.push_back(variant.value<QGeoCoordinate>());
}
}
/*!
* \fn void WimaAreaData::setPath(const QList<QGeoCoordinate> &coordinateList)
*
* Sets the path member to \a coordinateList by copying all entries of \a
* coordinateList. Emits the \c pathChanged() signal.
*/
void WimaAreaData::setPath(const QList<QGeoCoordinate> &coordinateList) {
if (_list != coordinateList) {
_setPathImpl(coordinateList);
emit pathChanged();
}
}
bool operator==(const WimaAreaData &m1, const WimaArea &m2) {
return m1.path() == m2.path() && m1.center() == m2.center();
}
bool operator!=(const WimaAreaData &m1, const WimaArea &m2) {
return !operator==(m1, m2);
}
bool operator==(const WimaArea &m1, const WimaAreaData &m2) { return m2 == m1; }
bool operator!=(const WimaArea &m1, const WimaAreaData &m2) {
return !operator==(m2, m1);
}
/*!
* \class WimaArea::WimaAreaData
* \brief Class to store and exchange data of a \c WimaArea Object.
* Class to store and exchange data of a \c WimaArea Object. In contrast to \c
* WimaArea this class does not uses the QGC Fact System. It is designed to
* exchange data between the \c WimaPlaner and the \c WimaController class. And
* it is the base class for any derived data objects
*
* \sa WimaArea
*/
src/Wima/Geometry/WimaAreaData.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QObject>
#include "QGeoCoordinate"
#include "WimaArea.h"
class WimaAreaData
: public QObject // Abstract class for all WimaAreaData derived objects
{
Q_OBJECT
public:
WimaAreaData(QObject *parent = nullptr);
Q_PROPERTY(const QVariantList path READ path NOTIFY pathChanged)
Q_PROPERTY(QString type READ type CONSTANT)
Q_PROPERTY(QString mapVisualQML READ mapVisualQML CONSTANT)
bool operator==(const WimaAreaData &data) const;
bool operator!=(const WimaAreaData &data) const;
virtual QString mapVisualQML(void) const = 0;
QVariantList path() const;
QGeoCoordinate center() const;
const QList<QGeoCoordinate> &coordinateList() const;
bool containsCoordinate(const QGeoCoordinate &coordinate) const;
virtual QString type() const = 0;
void append(const QGeoCoordinate &c);
void push_back(const QGeoCoordinate &c);
void clear();
signals:
void pathChanged();
void centerChanged();
public slots:
void setPath(const QList<QGeoCoordinate> &coordinateList);
void setPath(const QVariantList &coordinateList);
void setCenter(const QGeoCoordinate &center);
protected:
WimaAreaData(const WimaAreaData &otherData, QObject *parent);
WimaAreaData(const WimaArea &otherData, QObject *parent);
WimaAreaData &operator=(const WimaAreaData &otherData);
WimaAreaData &operator=(const WimaArea &otherData);
private:
void _setPathImpl(const QList<QGeoCoordinate> &coordinateList);
void _setPathImpl(const QVariantList &coordinateList);
// Member Variables
mutable QVariantList _path;
QList<QGeoCoordinate> _list;
QGeoCoordinate _center;
};
bool operator==(const WimaAreaData &m1, const WimaArea &m2);
bool operator!=(const WimaAreaData &m1, const WimaArea &m2);
bool operator==(const WimaArea &m1, const WimaAreaData &m2);
bool operator!=(const WimaArea &m1, const WimaAreaData &m2);
src/Wima/Geometry/WimaCorridor.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaCorridor.h"
const char* WimaCorridor::WimaCorridorName = "Corridor";
WimaCorridor::WimaCorridor(QObject *parent)
: WimaArea (parent)
{
init();
}
WimaCorridor::WimaCorridor(const WimaCorridor &other, QObject *parent)
: WimaArea (other, parent)
{
init();
}
/*!
* \overload operator=()
*
* Calls the inherited operator WimaArea::operator=().
*/
WimaCorridor &WimaCorridor::operator=(const WimaCorridor &other)
{
WimaArea::operator=(other);
return *this;
}
void WimaCorridor::saveToJson(QJsonObject &json)
{
this->WimaArea::saveToJson(json);
json[areaTypeName] = WimaCorridorName;
}
bool WimaCorridor::loadFromJson(const QJsonObject &json, QString &errorString)
{
if ( this->WimaArea::loadFromJson(json, errorString)) {
bool retVal = true;
// code for loading here
return retVal;
} else {
qWarning() << errorString;
return false;
}
}
void WimaCorridor::init()
{
this->setObjectName(WimaCorridorName);
}
/*!
\class WimaCorridor
\brief Corridor (derived from \c WimaArea) connecting the \c WimaMeasurementArea and the \c WimaServiceArea.
*/
src/Wima/Geometry/WimaCorridor.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "WimaArea.h"
#include <QObject>
class WimaCorridor : public WimaArea {
Q_OBJECT
public:
WimaCorridor(QObject *parent = nullptr);
WimaCorridor(const WimaCorridor &other, QObject *parent = nullptr);
WimaCorridor &operator=(const WimaCorridor &other);
// Overrides from WimaPolygon
QString mapVisualQML(void) const { return "WimaCorridorMapVisual.qml"; }
QString editorQML(void) const { return "WimaCorridorEditor.qml"; }
// Methodes
void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString);
// static Members
static const char *WimaCorridorName;
signals:
public slots:
private:
void init();
};
src/Wima/Geometry/WimaMeasurementArea.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaMeasurementArea.h"
#include "QtConcurrentRun"
#include "SnakeTile.h"
#include "snake.h"
#include <boost/units/systems/si.hpp>
#include "QGCLoggingCategory.h"
#ifndef SNAKE_MAX_TILES
#define SNAKE_MAX_TILES 1000
#endif
QGC_LOGGING_CATEGORY(WimaMeasurementAreaLog, "WimaMeasurementAreaLog")
TileData::TileData() : tiles(this) {}
TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData &TileData::operator=(const TileData &other) {
this->tiles.clearAndDeleteContents();
for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) {
const auto *obj = other.tiles[i];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this));
} else {
qCWarning(WimaMeasurementAreaLog) << "TileData::operator=: nullptr";
}
}
this->tileCenterPoints = other.tileCenterPoints;
return *this;
}
bool TileData::operator==(const TileData &other) const {
if (this->tileCenterPoints == other.tileCenterPoints &&
this->tiles.count() == other.tiles.count()) {
for (int i = 0; i < other.tiles.count(); ++i) {
if (this->tiles[i] != other.tiles[i]) {
return false;
}
}
return true;
} else {
return false;
}
}
bool TileData::operator!=(const TileData &other) const {
return !this->operator==(other);
}
void TileData::clear() {
this->tiles.clearAndDeleteContents();
this->tileCenterPoints.clear();
}
size_t TileData::size() const {
if (tiles.count() == tileCenterPoints.size()) {
return tiles.count();
} else {
return 0;
}
}
const char *WimaMeasurementArea::settingsGroup = "MeasurementArea";
const char *WimaMeasurementArea::tileHeightName = "TileHeight";
const char *WimaMeasurementArea::tileWidthName = "TileWidth";
const char *WimaMeasurementArea::minTileAreaName = "MinTileAreaPercent";
const char *WimaMeasurementArea::showTilesName = "ShowTiles";
const char *WimaMeasurementArea::WimaMeasurementAreaName = "Measurement Area";
WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
: WimaArea(parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"),
this /* QObject parent */)),
_tileHeight(SettingsFact(settingsGroup, _metaDataMap[tileHeightName],
this /* QObject parent */)),
_tileWidth(SettingsFact(settingsGroup, _metaDataMap[tileWidthName],
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
init();
}
WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
QObject *parent)
: WimaArea(other, parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"),
this /* QObject parent */)),
_tileHeight(SettingsFact(settingsGroup, _metaDataMap[tileHeightName],
this /* QObject parent */)),
_tileWidth(SettingsFact(settingsGroup, _metaDataMap[tileWidthName],
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
init();
}
/*!
* \overload operator=()
*
* Calls the inherited operator WimaArea::operator=().
*/
WimaMeasurementArea &WimaMeasurementArea::
operator=(const WimaMeasurementArea &other) {
WimaArea::operator=(other);
return *this;
}
WimaMeasurementArea::~WimaMeasurementArea() {}
QString WimaMeasurementArea::mapVisualQML() const {
return QStringLiteral("WimaMeasurementAreaMapVisual.qml");
}
QString WimaMeasurementArea::editorQML() const {
return QStringLiteral("WimaMeasurementAreaEditor.qml");
}
Fact *WimaMeasurementArea::tileHeight() { return &_tileHeight; }
Fact *WimaMeasurementArea::tileWidth() { return &_tileWidth; }
Fact *WimaMeasurementArea::minTileArea() { return &_minTileAreaPercent; }
Fact *WimaMeasurementArea::showTiles() { return &_showTiles; }
QmlObjectListModel *WimaMeasurementArea::tiles() {
return &this->_tileData.tiles;
}
const QVector<int> &WimaMeasurementArea::progress() const {
return this->_progress;
}
QVector<int> WimaMeasurementArea::progressQml() const {
return this->_progress;
}
const QmlObjectListModel *WimaMeasurementArea::tiles() const {
return &this->_tileData.tiles;
}
const QVariantList &WimaMeasurementArea::tileCenterPoints() const {
return this->_tileData.tileCenterPoints;
}
const TileData &WimaMeasurementArea::tileData() const {
return this->_tileData;
}
int WimaMeasurementArea::maxTiles() const { return SNAKE_MAX_TILES; }
bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) {
if (ready()) {
this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble();
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
} else {
qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing.";
}
}
bool WimaMeasurementArea::loadFromJson(const QJsonObject &json,
QString &errorString) {
if (this->WimaArea::loadFromJson(json, errorString)) {
disableUpdate();
bool retVal = true;
if (!json.contains(tileHeightName) || !json[tileHeightName].isDouble()) {
errorString.append(tr("Could not load tile height!\n"));
retVal = false;
} else {
_tileHeight.setRawValue(json[tileHeightName].toDouble());
}
if (!json.contains(tileWidthName) || !json[tileWidthName].isDouble()) {
errorString.append(tr("Could not load tile width!\n"));
retVal = false;
} else {
_tileWidth.setRawValue(json[tileWidthName].toDouble());
}
if (!json.contains(minTileAreaName) || !json[minTileAreaName].isDouble()) {
errorString.append(tr("Could not load minimal tile area!\n"));
retVal = false;
} else {
_minTileAreaPercent.setRawValue(json[minTileAreaName].toDouble());
}
if (!json.contains(showTilesName) || !json[showTilesName].isBool()) {
errorString.append(tr("Could not load show tiles !\n"));
retVal = false;
} else {
_showTiles.setRawValue(json[showTilesName].toBool());
}
enableUpdate();
doUpdate();
return retVal;
} else {
return false;
}
}
bool WimaMeasurementArea::setProgress(const QVector<int> &p) {
if (ready()) {
if (p.size() == this->tiles()->count() && this->_progress != p) {
this->_progress = p;
emit progressChanged();
emit progressAccepted();
return true;
}
}
return false;
}
//!
//! \brief WimaMeasurementArea::doUpdate
//! \pre WimaMeasurementArea::deferUpdate must be called first, don't call
//! this function directly!
void WimaMeasurementArea::doUpdate() {
using namespace snake;
using namespace boost::units;
auto start = std::chrono::high_resolution_clock::now();
if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) {
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea;
// Check some conditions.
if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
this->count() >= 3 && this->isSimplePolygon()) {
setState(STATE::UPDATEING);
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
const auto minArea =
this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea;
auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
auto start = std::chrono::high_resolution_clock::now();
DataPtr pData(new TileData());
// Convert to ENU system.
QGeoCoordinate origin = polygon.first();
FPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU;
BoundingBox bbox;
std::string errorString;
// Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex);
}
pData->tiles.append(geoTile);
// Calculate center.
snake::FPoint center;
snake::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
}
}
pData->moveToThread(th);
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
return pData;
}); // QtConcurrent::run()
this->_watcher.setFuture(future);
}
}
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
}
void WimaMeasurementArea::deferUpdate() {
if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) {
qCDebug(WimaMeasurementAreaLog) << "defereUpdate(): defer update.";
if (this->_state == STATE::IDLE) {
this->_progress.clear();
this->_tileData.clear();
emit this->progressChanged();
emit this->tilesChanged();
}
this->setState(STATE::DEFERED);
this->_timer.start(100);
} else if (this->_state == STATE::UPDATEING) {
qCDebug(WimaMeasurementAreaLog) << "defereUpdate(): restart.";
setState(STATE::RESTARTING);
}
}
void WimaMeasurementArea::storeTiles() {
auto start = std::chrono::high_resolution_clock::now();
if (this->_state == STATE::UPDATEING) {
qCDebug(WimaMeasurementAreaLog) << "storeTiles(): update.";
this->_tileData = *this->_watcher.result();
// This is expensive. Drawing tiles is expensive too.
this->_progress = QVector<int>(this->_tileData.tiles.count(), 0);
this->progressChanged();
emit this->tilesChanged();
setState(STATE::IDLE);
} else if (this->_state == STATE::RESTARTING) {
qCDebug(WimaMeasurementAreaLog) << "storeTiles(): restart.";
doUpdate();
} else if (this->_state == STATE::STOP) {
qCDebug(WimaMeasurementAreaLog) << "storeTiles(): stop.";
}
qCDebug(WimaMeasurementAreaLog)
<< "storeTiles() execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
}
void WimaMeasurementArea::disableUpdate() {
setState(STATE::IDLE);
this->_timer.stop();
}
void WimaMeasurementArea::enableUpdate() {
if (this->_state == STATE::STOP) {
setState(STATE::IDLE);
}
}
void WimaMeasurementArea::init() {
this->setObjectName(WimaMeasurementAreaName);
connect(&this->_tileHeight, &Fact::rawValueChanged, this,
&WimaMeasurementArea::deferUpdate);
connect(&this->_tileWidth, &Fact::rawValueChanged, this,
&WimaMeasurementArea::deferUpdate);
connect(&this->_minTileAreaPercent, &Fact::rawValueChanged, this,
&WimaMeasurementArea::deferUpdate);
connect(this, &WimaArea::pathChanged, this,
&WimaMeasurementArea::deferUpdate);
this->_timer.setSingleShot(true);
connect(&this->_timer, &QTimer::timeout, this,
&WimaMeasurementArea::doUpdate);
connect(&this->_watcher,
&QFutureWatcher<std::unique_ptr<QmlObjectListModel>>::finished, this,
&WimaMeasurementArea::storeTiles);
}
void WimaMeasurementArea::setState(WimaMeasurementArea::STATE s) {
if (this->_state != s) {
auto oldState = this->_state;
this->_state = s;
if (s == STATE::IDLE || oldState == STATE::IDLE) {
emit readyChanged();
}
}
}
/*!
* \class WimaMeasurementArea
* \brief Class defining the area inside which the actual drone measurements
* are performed.
*
* \sa WimaArea, WimaController, WimaPlaner
*/
src/Wima/Geometry/WimaMeasurementArea.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QFutureWatcher>
#include <QObject>
#include <QTimer>
#include "WimaArea.h"
#include "SettingsFact.h"
class TileData : public QObject {
public:
QmlObjectListModel tiles;
QVariantList tileCenterPoints;
TileData();
~TileData();
TileData &operator=(const TileData &other);
bool operator==(const TileData &other) const;
bool operator!=(const TileData &other) const;
void clear();
std::size_t size() const;
};
class WimaMeasurementArea : public WimaArea {
Q_OBJECT
enum class STATE { IDLE, DEFERED, UPDATEING, RESTARTING, STOP };
public:
WimaMeasurementArea(QObject *parent = nullptr);
WimaMeasurementArea(const WimaMeasurementArea &other,
QObject *parent = nullptr);
WimaMeasurementArea &operator=(const WimaMeasurementArea &other);
~WimaMeasurementArea();
Q_PROPERTY(Fact *tileHeight READ tileHeight CONSTANT)
Q_PROPERTY(Fact *tileWidth READ tileWidth CONSTANT)
Q_PROPERTY(Fact *minTileArea READ minTileArea CONSTANT)
Q_PROPERTY(Fact *showTiles READ showTiles CONSTANT)
Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tilesChanged)
Q_PROPERTY(int maxTiles READ maxTiles NOTIFY maxTilesChanged)
Q_PROPERTY(QVector<int> progress READ progressQml NOTIFY progressChanged)
// Overrides from WimaPolygon
QString mapVisualQML(void) const;
QString editorQML(void) const;
// Property getters.
Fact *tileHeight();
Fact *tileWidth();
Fact *minTileArea();
Fact *showTiles();
QmlObjectListModel *tiles();
const QVector<int> &progress() const;
QVector<int> progressQml() const;
const QmlObjectListModel *tiles() const;
const QVariantList &tileCenterPoints() const; // List of QGeoCoordinate
const TileData &tileData() const;
int maxTiles() const;
bool ready() const;
// Member Methodes
void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString);
// Static Variables
static const char *settingsGroup;
static const char *tileHeightName;
static const char *tileWidthName;
static const char *minTileAreaName;
static const char *showTilesName;
static const char *WimaMeasurementAreaName;
signals:
void tilesChanged();
void maxTilesChanged();
void progressChanged();
void progressAccepted();
void progressNotAccepted();
void readyChanged();
public slots:
bool setProgress(const QVector<int> &p);
private slots:
void doUpdate();
void deferUpdate();
void storeTiles();
void disableUpdate();
void enableUpdate();
private:
// Member Methodes
void init();
void setState(STATE s);
// Members
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _tileHeight;
SettingsFact _tileWidth;
SettingsFact _minTileAreaPercent; // 0..100
SettingsFact _showTiles;
QVector<int> _progress;
// Tile stuff.
// Tile stuff.
mutable QTimer _timer;
using DataPtr = std::shared_ptr<TileData>;
mutable STATE _state;
mutable TileData _tileData;
mutable QFutureWatcher<DataPtr> _watcher;
};
src/Wima/Geometry/WimaMeasurementAreaData.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaMeasurementAreaData.h"
#include "SnakeTile.h"
const char *WimaMeasurementAreaData::typeString = "WimaMeasurementAreaData";
WimaMeasurementAreaData::WimaMeasurementAreaData(QObject *parent)
: WimaAreaData(parent) {}
WimaMeasurementAreaData::WimaMeasurementAreaData(
const WimaMeasurementAreaData &other, QObject *parent)
: WimaAreaData(parent) {
*this = other;
}
WimaMeasurementAreaData::WimaMeasurementAreaData(
const WimaMeasurementArea &other, QObject *parent)
: WimaAreaData(parent) {
*this = other;
}
bool WimaMeasurementAreaData::
operator==(const WimaMeasurementAreaData &other) const {
return this->WimaAreaData::operator==(other) &&
this->_tileData == other.tileData() &&
this->_progress == other.progress() &&
this->center() == other.center();
}
bool WimaMeasurementAreaData::
operator!=(const WimaMeasurementAreaData &other) const {
return !(*this == other);
}
void WimaMeasurementAreaData::setTileData(const TileData &d) {
if (this->_tileData != d) {
this->_tileData = d;
this->_progress.fill(0, d.size());
emit progressChanged();
emit tileDataChanged();
}
}
void WimaMeasurementAreaData::setProgress(const QVector<int> &d) {
if (this->_progress != d && d.size() == this->_tileData.tiles.count()) {
this->_progress = d;
emit progressChanged();
}
}
/*!
* \overload operator=();
*
* Assigns \a other to the invoking object.
*/
WimaMeasurementAreaData &WimaMeasurementAreaData::
operator=(const WimaMeasurementAreaData &other) {
WimaAreaData::operator=(other);
setTileData(other._tileData);
setProgress(other._progress);
return *this;
}
/*!
* \overload operator=();
*
* Assigns \a other to the invoking object.
*/
WimaMeasurementAreaData &WimaMeasurementAreaData::
operator=(const WimaMeasurementArea &other) {
WimaAreaData::operator=(other);
if (other.ready()) {
setTileData(other.tileData());
setProgress(other.progress());
} else {
qWarning() << "WimaMeasurementAreaData::operator=(): WimaMeasurementArea "
"not ready.";
}
return *this;
}
QString WimaMeasurementAreaData::mapVisualQML() const {
return QStringLiteral("WimaMeasurementAreaDataVisual.qml");
}
QString WimaMeasurementAreaData::type() const { return this->typeString; }
QmlObjectListModel *WimaMeasurementAreaData::tiles() {
return &this->_tileData.tiles;
}
const QmlObjectListModel *WimaMeasurementAreaData::tiles() const {
return &this->_tileData.tiles;
}
const QVariantList &WimaMeasurementAreaData::tileCenterPoints() const {
return this->_tileData.tileCenterPoints;
}
QVariantList &WimaMeasurementAreaData::tileCenterPoints() {
return this->_tileData.tileCenterPoints;
}
const TileData &WimaMeasurementAreaData::tileData() const {
return this->_tileData;
}
TileData &WimaMeasurementAreaData::tileData() { return this->_tileData; }
const QVector<int> &WimaMeasurementAreaData::progress() const {
return this->_progress;
}
QVector<int> &WimaMeasurementAreaData::progress() { return this->_progress; }
bool operator==(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2) {
return operator==(*static_cast<const WimaAreaData *>(&m1),
*static_cast<const WimaArea *>(&m2)) &&
m1.tileData() == m2.tileData() && m1.progress() == m2.progress();
}
bool operator!=(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2) {
return !(m1 == m2);
}
bool operator==(const WimaMeasurementArea &m1,
const WimaMeasurementAreaData &m2) {
return m2 == m1;
}
bool operator!=(const WimaMeasurementArea &m1,
const WimaMeasurementAreaData &m2) {
return m2 != m1;
}
src/Wima/Geometry/WimaMeasurementAreaData.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QGeoCoordinate>
#include <QObject>
#include "WimaAreaData.h"
#include "WimaMeasurementArea.h"
class WimaMeasurementAreaData : public WimaAreaData {
Q_OBJECT
public:
WimaMeasurementAreaData(QObject *parent = nullptr);
WimaMeasurementAreaData(const WimaMeasurementAreaData &other,
QObject *parent = nullptr);
WimaMeasurementAreaData(const WimaMeasurementArea &other,
QObject *parent = nullptr);
WimaMeasurementAreaData &operator=(const WimaMeasurementAreaData &other);
WimaMeasurementAreaData &operator=(const WimaMeasurementArea &other);
Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tileDataChanged)
Q_PROPERTY(QVector<int> progress READ progress NOTIFY progressChanged)
virtual QString mapVisualQML() const override;
bool operator==(const WimaMeasurementAreaData &other) const;
bool operator!=(const WimaMeasurementAreaData &other) const;
// Property setters.
void setTileData(const TileData &d);
void setProgress(const QVector<int> &d);
// Property getters.
QString type() const;
WimaMeasurementAreaData *Clone() const {
return new WimaMeasurementAreaData(*this);
}
QmlObjectListModel *tiles();
const QmlObjectListModel *tiles() const;
const QVariantList &tileCenterPoints() const;
QVariantList &tileCenterPoints();
const TileData &tileData() const;
TileData &tileData();
const QVector<int> &progress() const;
QVector<int> &progress();
static const char *typeString;
signals:
void tileDataChanged();
void progressChanged();
private:
TileData _tileData;
QVector<int> _progress;
};
bool operator==(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2);
bool operator!=(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2);
bool operator==(const WimaMeasurementArea &m1,
const WimaMeasurementAreaData &m2);
bool operator!=(const WimaMeasurementArea &m1,
const WimaMeasurementAreaData &m2);
src/Wima/Geometry/WimaServiceArea.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaServiceArea.h"
#include "QGCLoggingCategory.h"
#include "QGCQGeoCoordinate.h"
QGC_LOGGING_CATEGORY(WimaServiceAreaLog, "WimaServiceAreaLog")
const char *WimaServiceArea::wimaServiceAreaName = "Service Area";
const char *WimaServiceArea::depotLatitudeName = "DepotLatitude";
const char *WimaServiceArea::depotLongitudeName = "DepotLongitude";
const char *WimaServiceArea::depotAltitudeName = "DepotAltitude";
WimaServiceArea::WimaServiceArea(QObject *parent) : WimaArea(parent) { init(); }
WimaServiceArea::WimaServiceArea(const WimaServiceArea &other, QObject *parent)
: WimaArea(other, parent), _depot(other.depot()) {
init();
}
/*!
* \overload operator=()
*
* Calls the inherited operator WimaArea::operator=().
*/
WimaServiceArea &WimaServiceArea::operator=(const WimaServiceArea &other) {
WimaArea::operator=(other);
this->setDepot(other.depot());
return *this;
}
const QGeoCoordinate &WimaServiceArea::depot() const { return _depot; }
QGeoCoordinate WimaServiceArea::depotQml() const { return _depot; }
bool WimaServiceArea::setDepot(const QGeoCoordinate &coordinate) {
if (_depot.latitude() != coordinate.latitude() ||
_depot.longitude() != coordinate.longitude()) {
if (this->containsCoordinate(coordinate)) {
_depot = coordinate;
_depot.setAltitude(0);
emit depotChanged();
return true;
}
}
return false;
}
void WimaServiceArea::saveToJson(QJsonObject &json) {
this->WimaArea::saveToJson(json);
json[areaTypeName] = wimaServiceAreaName;
json[depotLatitudeName] = _depot.latitude();
json[depotLongitudeName] = _depot.longitude();
json[depotAltitudeName] = _depot.altitude();
}
bool WimaServiceArea::loadFromJson(const QJsonObject &json,
QString &errorString) {
bool retVal = false;
if (this->WimaArea::loadFromJson(json, errorString)) {
double lat = 0;
if (json.contains(depotLatitudeName) &&
json[depotLatitudeName].isDouble()) {
lat = json[depotLatitudeName].toDouble();
double lon = 0;
if (json.contains(depotLongitudeName) &&
json[depotLongitudeName].isDouble()) {
lon = json[depotLongitudeName].toDouble();
double alt = 0;
if (json.contains(depotAltitudeName) &&
json[depotAltitudeName].isDouble()) {
alt = json[depotAltitudeName].toDouble();
this->setDepot(QGeoCoordinate(lat, lon, alt));
retVal = true;
} else {
errorString = "Not able to load depot altitude.";
}
} else {
errorString = "Not able to load depot longitude.";
}
} else {
errorString = "Not able to load depot latitude.";
}
retVal = true;
}
return retVal;
}
void WimaServiceArea::init() {
this->setObjectName(wimaServiceAreaName);
connect(this, &WimaArea::pathChanged, [this] {
if (!this->_depot.isValid() || !this->containsCoordinate(this->_depot)) {
if (this->containsCoordinate(this->center())) {
// Use center.
this->setDepot(this->center());
} else if (this->_depot.isValid()) {
// Use nearest coordinate.
auto minDist = std::numeric_limits<double>::infinity();
int minIndex = 0;
for (int idx = 0; idx < this->pathModel().count(); ++idx) {
const QObject *obj = this->pathModel()[idx];
const auto *vertex = qobject_cast<const QGCQGeoCoordinate *>(obj);
if (vertex != nullptr) {
auto d = vertex->coordinate().distanceTo(this->_depot);
if (d < minDist) {
minDist = d;
minIndex = idx;
}
} else {
qCCritical(WimaServiceAreaLog) << "init(): nullptr catched!";
}
}
this->setDepot(this->pathModel().value<QGCQGeoCoordinate *>(minIndex)->coordinate());
} else if (this->pathModel().count() > 0) {
// Use first coordinate.
this->setDepot(this->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate());
}
}
});
}
src/Wima/Geometry/WimaServiceArea.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "WimaArea.h"
#include "WimaTrackerPolyline.h"
#include <QObject>
class WimaServiceArea : public WimaArea {
Q_OBJECT
public:
WimaServiceArea(QObject *parent = nullptr);
WimaServiceArea(const WimaServiceArea &other, QObject *parent);
WimaServiceArea &operator=(const WimaServiceArea &other);
Q_PROPERTY(
QGeoCoordinate depot READ depotQml WRITE setDepot NOTIFY depotChanged)
// Overrides from WimaPolygon
QString mapVisualQML(void) const { return "WimaServiceAreaMapVisual.qml"; }
QString editorQML(void) const { return "WimaServiceAreaEditor.qml"; }
// Property acessors
const QGeoCoordinate &depot(void) const;
QGeoCoordinate depotQml(void) const;
// Member Methodes
void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString);
// static Members
static const char *wimaServiceAreaName;
static const char *depotLatitudeName;
static const char *depotLongitudeName;
static const char *depotAltitudeName;
signals:
void depotChanged(void);
public slots:
bool setDepot(const QGeoCoordinate &coordinate);
private:
// Member Methodes
void init();
// Members
QGeoCoordinate _depot;
};
src/Wima/Geometry/WimaServiceAreaData.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaServiceAreaData.h"
const char *WimaServiceAreaData::typeString = "WimaServiceAreaData";
WimaServiceAreaData::WimaServiceAreaData(QObject *parent)
: WimaAreaData(parent) {}
WimaServiceAreaData::WimaServiceAreaData(const WimaServiceAreaData &other,
QObject *parent)
: WimaAreaData(other, parent), _depot(other._depot) {}
WimaServiceAreaData::WimaServiceAreaData(const WimaServiceArea &other,
QObject *parent)
: WimaAreaData(other, parent), _depot(other.depot()) {}
WimaServiceAreaData &WimaServiceAreaData::
operator=(const WimaServiceAreaData &otherData) {
WimaAreaData::operator=(otherData);
this->setDepot(otherData.depot());
return *this;
}
WimaServiceAreaData &WimaServiceAreaData::
operator=(const WimaServiceArea &otherArea) {
WimaAreaData::operator=(otherArea);
this->setDepot(otherArea.depot());
return *this;
}
QString WimaServiceAreaData::mapVisualQML() const {
return QStringLiteral("WimaServiceAreaDataVisual.qml");
}
/*!
* \fn const QGeoCoordinate &WimaServiceAreaData::takeOffPosition() const
* Returns a constant reference to the takeOffPosition.
*
*/
const QGeoCoordinate &WimaServiceAreaData::depot() const { return _depot; }
QString WimaServiceAreaData::type() const { return this->typeString; }
/*!
* \fn void WimaServiceAreaData::setTakeOffPosition(const QGeoCoordinate
* &newCoordinate) Sets the takeoff position to the \a newCoordinate and emits
* the takeOffPositionChanged() signal, if newCoordinate differs from the member
* value.
*
*/
void WimaServiceAreaData::setDepot(const QGeoCoordinate &newCoordinate) {
if (_depot != newCoordinate) {
_depot = newCoordinate;
emit depotChanged(_depot);
}
}
/*!
* \class WimaAreaData::WimaServiceAreaData
* \brief Class to store and exchange data of a \c WimaServiceArea Object.
* Class to store and exchange data of a \c WimaServiceArea Object. In contrast
* to \c WimaServiceArea this class does not provied any interface to a grafical
* user interface, neiter it uses the QGC Fact System. It is designed to
* exchange data between the \c WimaPlaner and the \c WimaController class. And
* it is the derived from WimaAreaData.
*
* \sa WimaServiceArea, WimaAreaData
*/
src/Wima/Geometry/WimaServiceAreaData.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "QGeoCoordinate"
#include <QObject>
#include "WimaAreaData.h"
#include "WimaServiceArea.h"
class WimaServiceAreaData : public WimaAreaData {
Q_OBJECT
public:
WimaServiceAreaData(QObject *parent = nullptr);
WimaServiceAreaData(const WimaServiceAreaData &other,
QObject *parent = nullptr);
WimaServiceAreaData(const WimaServiceArea &other, QObject *parent = nullptr);
WimaServiceAreaData &operator=(const WimaServiceAreaData &otherData);
WimaServiceAreaData &operator=(const WimaServiceArea &otherArea);
virtual QString mapVisualQML() const override;
const QGeoCoordinate &depot() const;
QString type() const;
WimaServiceAreaData *Clone() const { return new WimaServiceAreaData(); }
static const char *typeString;
signals:
void depotChanged(const QGeoCoordinate &other);
public slots:
void setDepot(const QGeoCoordinate &newCoordinate);
private:
// see WimaServieArea.h for explanation
QGeoCoordinate _depot;
};
src/Wima/Geometry/json/WimaArea.SettingsGroup.json deleted 100644 → 0
View file @ 458ab210
[
{
"name": "BorderPolygonOffset",
"shortDescription": "The distance between the measurement area and it's surrounding polygon",
"type": "double",
"units": "m",
"min": 0,
"decimalPlaces": 1,
"defaultValue": 5
},
{
"name": "ShowBorderPolygon",
"shortDescription": "Border polygon will be displayed if checked.",
"type": "bool",
"defaultValue": true
}
]
src/Wima/Geometry/json/WimaMeasurementArea.SettingsGroup.json deleted 100644 → 0
View file @ 458ab210
[
{
"name": "TileHeight",
"shortDescription": "The height of a tile",
"type": "double",
"units": "m",
"min": 0.3,
"decimalPlaces": 2,
"defaultValue": 5
},
{
"name": "TileWidth",
"shortDescription": "The height of a tile",
"type": "double",
"units": "m",
"min": 0.3,
"decimalPlaces": 2,
"defaultValue": 5
},
{
"name": "MinTileAreaPercent",
"shortDescription": "The minimal allowed area in percent (of width*height).",
"type": "double",
"units": "%",
"min": 0,
"max": 100,
"decimalPlaces": 2,
"defaultValue": 20
},
{
"name": "ShowTiles",
"shortDescription": "Show tiles",
"type": "bool",
"defaultValue": true
},
{
"name": "BorderPolygonOffset",
"shortDescription": "The distance between the measurement area and it's enclosing polygon",
"type": "double",
"units": "m",
"min": 0,
"decimalPlaces": 1,
"defaultValue": 5
}
]
src/Wima/Geometry/testplanimetrycalculus.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "PlanimetryCalculus.h"
#include <QPointF>
#include <QLineF>
namespace TestPlanimetryCalculus {
void test();
}
src/Wima/OptimisationTools.cc deleted 100644 → 0
View file @ 458ab210
#include "OptimisationTools.h"
namespace OptimisationTools {
namespace {
} // end anonymous namespace
bool dijkstraAlgorithm(const int numElements, int startIndex, int endIndex, QVector<int> &elementPath, std::function<double (const int, const int)> distanceDij)
{
if ( numElements < 0
|| startIndex < 0
|| endIndex < 0
|| startIndex >= numElements
|| endIndex >= numElements
|| endIndex == startIndex) {
return false;
}
// Node struct
// predecessorIndex is the index of the predecessor node (nodeList[predecessorIndex])
// distance is the distance between the node and the start node
// node number is stored by the position in nodeList
struct Node{
int predecessorIndex = -1;
double distance = std::numeric_limits<qreal>::infinity();
};
// The list with all Nodes (elements)
QVector<Node> nodeList(numElements);
// This list will be initalized with indices referring to the elements of nodeList.
// Elements will be successively remove during the execution of the Dijkstra Algorithm.
QVector<int> workingSet(numElements);
//append elements to node list
for (int i = 0; i < numElements; ++i) workingSet[i] = i;
nodeList[startIndex].distance = 0;
// qDebug() << "nodeList" ;
// for (auto node : nodeList) {
// qDebug() << "predecessor: " << node.predecessorIndex;
// qDebug() << "distance: " << node.distance;
// }
// qDebug() << "workingSet";
// for (auto node : workingSet) {
// qDebug() << "index: " << node;
// }
// Dijkstra Algorithm
// https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
while (workingSet.size() > 0) {
// serach Node with minimal distance
double minDist = std::numeric_limits<qreal>::infinity();
int minDistIndex_WS = -1; // WS = workinSet
for (int i = 0; i < workingSet.size(); ++i) {
const int nodeIndex = workingSet.at(i);
const double dist = nodeList.at(nodeIndex).distance;
if (dist < minDist) {
minDist = dist;
minDistIndex_WS = i;
}
}
if (minDistIndex_WS == -1)
return false;
int indexU_NL = workingSet.takeAt(minDistIndex_WS); // NL = nodeList
if (indexU_NL == endIndex) // shortest path found
break;
const double distanceU = nodeList.at(indexU_NL).distance;
//update distance
for (int i = 0; i < workingSet.size(); ++i) {
int indexV_NL = workingSet[i]; // NL = nodeList
Node* v = &nodeList[indexV_NL];
double dist = distanceDij(indexU_NL, indexV_NL);
// is ther an alternative path which is shorter?
double alternative = distanceU + dist;
if (alternative < v->distance) {
v->distance = alternative;
v->predecessorIndex = indexU_NL;
}
}
}
// end Djikstra Algorithm
// reverse assemble path
int e = endIndex;
while (1) {
if (e == -1) {
if (elementPath[0] == startIndex)// check if starting point was reached
break;
return false;
}
elementPath.prepend(e);
//Update Node
e = nodeList[e].predecessorIndex;
}
return true;
}
} // end OptimisationTools namespace
src/Wima/OptimisationTools.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QObject>
#include <QDebug>
#include <functional>
#include <QVector>
namespace OptimisationTools {
bool dijkstraAlgorithm(const int numElements, int startIndex, int endIndex, QVector<int> &elementPath, std::function<double(const int, const int)> distanceDij);
}
src/Wima/RoutingThread.cpp deleted 100644 → 0
View file @ 458ab210
#include "RoutingThread.h"
// std
#include <chrono>
// Qt
#include <QDebug>
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(RoutingThreadLog, "RoutingThreadLog")
RoutingThread::RoutingThread(QObject *parent)
: QThread(parent), _calculating(false), _stop(false), _restart(false) {
static std::once_flag flag;
std::call_once(flag,
[] { qRegisterMetaType<PtrRoutingData>("PtrRoutingData"); });
}
RoutingThread::~RoutingThread() {
this->_stop = true;
Lock lk(this->_mutex);
this->_restart = true;
this->_cv.notify_one();
lk.unlock();
this->wait();
}
bool RoutingThread::calculating() const { return this->_calculating; }
void RoutingThread::route(const RoutingParameter &par,
const Generator &generator) {
// Sample input.
Lock lk(this->_mutex);
this->_par = par;
this->_generator = generator;
lk.unlock();
if (!this->isRunning()) {
this->start();
} else {
Lock lk(this->_mutex);
this->_restart = true;
this->_cv.notify_one();
}
}
void RoutingThread::run() {
qCDebug(RoutingThreadLog) << "run(): thread start.";
while (!this->_stop) {
qCDebug(RoutingThreadLog) << "run(): calculation "
"started.";
// Copy input.
auto start = std::chrono::high_resolution_clock::now();
this->_calculating = true;
emit calculatingChanged();
Lock lk(this->_mutex);
auto par = this->_par;
auto generator = this->_generator;
lk.unlock();
auto safeAreaENU = par.safeArea;
auto numRuns = par.numRuns;
auto numSolutionsPerRun = par.numSolutions;
PtrRoutingData pRouteData(new RoutingData());
auto &transectsENU = pRouteData->transects;
// Generate transects.
if (generator(transectsENU)) {
// Check if generation was successful.
if (transectsENU.size() == 0) {
qCDebug(RoutingThreadLog) << "run(): "
"not able to generate transects.";
} else {
// Prepare data for routing.
auto &solutionVector = pRouteData->solutionVector;
snake::RouteParameter snakePar;
snakePar.numSolutionsPerRun = numSolutionsPerRun;
snakePar.numRuns = numRuns;
// Set time limit to 10 min.
const auto maxRoutingTime = std::chrono::minutes(10);
const auto routingEnd =
std::chrono::high_resolution_clock::now() + maxRoutingTime;
const auto &restart = this->_restart;
snakePar.stop = [&restart, routingEnd] {
bool expired = std::chrono::high_resolution_clock::now() > routingEnd;
return restart || expired;
};
// Route transects.
bool success =
snake::route(safeAreaENU, transectsENU, solutionVector, snakePar);
// Check if routing was successful.
if ((!success || solutionVector.size() < 1) && !this->_restart) {
qCDebug(RoutingThreadLog) << "run(): "
"routing failed. "
<< snakePar.errorString.c_str();
} else if (this->_restart) {
qCDebug(RoutingThreadLog) << "run(): "
"restart requested.";
} else {
// Notify main thread.
emit result(pRouteData);
qCDebug(RoutingThreadLog) << "run(): "
"concurrent update success.";
}
}
} // end calculation
else {
qCDebug(RoutingThreadLog) << "run(): generator() failed.";
}
qCDebug(RoutingThreadLog)
<< "run(): execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
// Signal calulation end and set thread to sleep.
this->_calculating = false;
emit calculatingChanged();
Lock lk2(this->_mutex);
if (!this->_restart) {
this->_cv.wait(lk2, [this] { return this->_restart.load(); });
}
this->_restart = false;
} // main loop
qCDebug(RoutingThreadLog) << "run(): thread end.";
}
src/Wima/RoutingThread.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QGeoCoordinate>
#include <QSharedPointer>
#include <QThread>
#include "snake.h"
#include <atomic>
#include <condition_variable>
#include <functional>
#include <mutex>
struct RoutingData {
snake::Transects transects;
std::vector<snake::Solution> solutionVector;
std::string errorString;
};
struct RoutingParameter {
RoutingParameter() : numSolutions(1), numRuns(1) {}
snake::FPolygon safeArea;
std::size_t numSolutions;
std::size_t numRuns;
};
//!
//! \brief The CSWorker class
//! \note Don't call QThread::start, QThread::quit, etc. onyl use Worker
//! members!
class RoutingThread : public QThread {
Q_OBJECT
using Lock = std::unique_lock<std::mutex>;
public:
using PtrRoutingData = shared_ptr<RoutingData>;
using Generator = std::function<bool(snake::Transects &)>;
using Consumer = std::function<void(const RoutingData &)>;
RoutingThread(QObject *parent = nullptr);
~RoutingThread() override;
bool calculating() const;
public slots:
void route(const RoutingParameter &par, const Generator &generator);
signals:
void result(PtrRoutingData pTransects);
void calculatingChanged();
protected:
void run() override;
private:
mutable std::mutex _mutex;
mutable std::condition_variable _cv;
// Internal data
RoutingParameter _par;
Generator _generator; // transect generator
// State
std::atomic_bool _calculating;
std::atomic_bool _stop;
std::atomic_bool _restart;
};
src/Wima/Snake/CircularGenerator.cpp deleted 100644 → 0
View file @ 458ab210
#include "CircularGenerator.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(CircularGeneratorLog, "CircularGeneratorLog")
#define CLIPPER_SCALE 1000000
#include "Wima/Geometry/GenericCircle.h"
#include "clipper/clipper.hpp"
using namespace ClipperLib;
template <> inline auto get<0>(const IntPoint &p) { return p.X; }
template <> inline auto get<1>(const IntPoint &p) { return p.Y; }
#include "SnakeTile.h"
#include "Wima/RoutingThread.h"
namespace routing {
bool circularTransects(const snake::FPoint &reference,
const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length deltaR, snake::Angle deltaAlpha,
snake::Length minLength, snake::Transects &transects);
const char *CircularGenerator::settingsGroup = "CircularGenerator";
const char *CircularGenerator::distanceName = "TransectDistance";
const char *CircularGenerator::deltaAlphaName = "DeltaAlpha";
const char *CircularGenerator::minLengthName = "MinLength";
const char *CircularGenerator::refPointLatitudeName = "ReferencePointLat";
const char *CircularGenerator::refPointLongitudeName = "ReferencePointLong";
const char *CircularGenerator::refPointAltitudeName = "ReferencePointAlt";
CircularGenerator::CircularGenerator(QObject *parent)
: CircularGenerator(nullptr, parent) {}
CircularGenerator::CircularGenerator(GeneratorBase::Data d, QObject *parent)
: GeneratorBase(d, parent), _connectionsEstablished(false),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/CircularGenerator.SettingsGroup.json"), this)),
_distance(settingsGroup, _metaDataMap[distanceName]),
_deltaAlpha(settingsGroup, _metaDataMap[deltaAlphaName]),
_minLength(settingsGroup, _metaDataMap[minLengthName]) {
establishConnections();
}
QString CircularGenerator::editorQml() {
return QStringLiteral("CircularGeneratorEditor.qml");
}
QString CircularGenerator::mapVisualQml() {
return QStringLiteral("CircularGeneratorMapVisual.qml");
}
QString CircularGenerator::name() {
return QStringLiteral("Circular Generator");
}
QString CircularGenerator::abbreviation() { return QStringLiteral("C. Gen."); }
bool CircularGenerator::get(Generator &generator) {
if (this->_d) {
if (this->_d->isValid()) {
// Prepare data.
auto origin = this->_d->origin();
origin.setAltitude(0);
if (!origin.isValid()) {
qCDebug(CircularGeneratorLog) << "get(): origin invalid." << origin;
return false;
}
auto ref = this->_reference;
ref.setAltitude(0);
if (!ref.isValid()) {
qCDebug(CircularGeneratorLog) << "get(): reference invalid." << ref;
return false;
}
snake::FPoint reference;
snake::toENU(origin, ref, reference);
auto geoPolygon = this->_d->measurementArea().coordinateList();
for (auto &v : geoPolygon) {
if (v.isValid()) {
v.setAltitude(0);
} else {
qCDebug(CircularGeneratorLog) << "get(): measurement area invalid.";
for (const auto &w : geoPolygon) {
qCDebug(CircularGeneratorLog) << w;
}
return false;
}
}
auto pPolygon = std::make_shared<snake::FPolygon>();
snake::areaToEnu(origin, geoPolygon, *pPolygon);
// Progress and tiles.
const auto &progress = this->_d->measurementArea().progress();
const auto *tiles = this->_d->measurementArea().tiles();
auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
const auto *obj = (*tiles)[int(i)];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
pTiles->push_back(std::move(tileENU));
} else {
qCDebug(CircularGeneratorLog)
<< "get(): progress.size() != tiles->count().";
return false;
}
}
}
} else {
qCDebug(CircularGeneratorLog)
<< "get(): progress.size() != tiles->count().";
return false;
}
auto geoDepot = this->_d->serviceArea().depot();
if (!geoDepot.isValid()) {
qCDebug(CircularGeneratorLog) << "get(): depot invalid." << geoDepot;
return false;
}
snake::FPoint depot;
snake::toENU(origin, geoDepot, depot);
// Fetch transect parameter.
auto distance =
snake::Length(this->_distance.rawValue().toDouble() * bu::si::meter);
auto minLength =
snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
auto alpha = snake::Angle(this->_deltaAlpha.rawValue().toDouble() *
bu::degree::degree);
generator = [reference, depot, pPolygon, pTiles, distance, alpha,
minLength](snake::Transects &transects) -> bool {
bool value = circularTransects(reference, *pPolygon, *pTiles, distance,
alpha, minLength, transects);
transects.insert(transects.begin(), snake::FLineString{depot});
return value;
};
return true;
} else {
qCDebug(CircularGeneratorLog) << "get(): data invalid.";
return false;
}
} else {
qCDebug(CircularGeneratorLog) << "get(): data member not set.";
return false;
}
}
QGeoCoordinate CircularGenerator::reference() const { return _reference; }
void CircularGenerator::setReference(const QGeoCoordinate &reference) {
if (_reference != reference) {
_reference = reference;
emit referenceChanged();
}
}
void CircularGenerator::resetReferenceIfInvalid() {
if (!this->_reference.isValid()) {
resetReference();
}
}
void CircularGenerator::resetReference() {
if (this->_d->measurementArea().center().isValid()) {
setReference(this->_d->measurementArea().center());
} else {
qCWarning(CircularGeneratorLog)
<< "measurement area center" << this->_d->measurementArea().center();
}
}
void CircularGenerator::establishConnections() {
if (this->_d && !this->_connectionsEstablished) {
connect(this->_d.get(), &AreaData::originChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::progressChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::tileDataChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::centerChanged, this,
&CircularGenerator::resetReferenceIfInvalid);
connect(&this->_d->measurementArea(), &WimaMeasurementAreaData::pathChanged,
this, &GeneratorBase::generatorChanged);
connect(&this->_d->serviceArea(), &WimaServiceAreaData::depotChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->joinedArea(), &WimaJoinedAreaData::pathChanged, this,
&GeneratorBase::generatorChanged);
connect(this->distance(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
connect(this->deltaAlpha(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
connect(this->minLength(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
connect(this, &CircularGenerator::referenceChanged, this,
&GeneratorBase::generatorChanged);
this->_connectionsEstablished = true;
}
}
void CircularGenerator::deleteConnections() {
if (this->_d && this->_connectionsEstablished) {
disconnect(this->_d.get(), &AreaData::originChanged, this,
&GeneratorBase::generatorChanged);
disconnect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::progressChanged, this,
&GeneratorBase::generatorChanged);
disconnect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::tileDataChanged, this,
&GeneratorBase::generatorChanged);
disconnect(&this->_d->measurementArea(), &WimaMeasurementAreaData::center,
this, &CircularGenerator::resetReferenceIfInvalid);
disconnect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::pathChanged, this,
&GeneratorBase::generatorChanged);
disconnect(&this->_d->serviceArea(), &WimaServiceAreaData::depotChanged,
this, &GeneratorBase::generatorChanged);
disconnect(&this->_d->joinedArea(), &WimaJoinedAreaData::pathChanged, this,
&GeneratorBase::generatorChanged);
disconnect(this->distance(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
disconnect(this->deltaAlpha(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
disconnect(this->minLength(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
disconnect(this, &CircularGenerator::referenceChanged, this,
&GeneratorBase::generatorChanged);
this->_connectionsEstablished = false;
}
}
Fact *CircularGenerator::distance() { return &_distance; }
Fact *CircularGenerator::deltaAlpha() { return &_deltaAlpha; }
Fact *CircularGenerator::minLength() { return &_minLength; }
bool circularTransects(const snake::FPoint &reference,
const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length deltaR, snake::Angle deltaAlpha,
snake::Length minLength, snake::Transects &transects) {
auto s1 = std::chrono::high_resolution_clock::now();
// Check preconitions
if (polygon.outer().size() >= 3) {
using namespace boost::units;
// Convert geo polygon to ENU polygon.
std::string error;
// Check validity.
if (!bg::is_valid(polygon, error)) {
qCDebug(CircularGeneratorLog) << "circularTransects(): "
"invalid polygon.";
qCDebug(CircularGeneratorLog) << error.c_str();
std::stringstream ss;
ss << bg::wkt(polygon);
qCDebug(CircularGeneratorLog) << ss.str().c_str();
} else {
// Calculate polygon distances and angles.
std::vector<snake::Length> distances;
distances.reserve(polygon.outer().size());
std::vector<snake::Angle> angles;
angles.reserve(polygon.outer().size());
//#ifdef DEBUG_CIRCULAR_SURVEY
// qCDebug(CircularGeneratorLog) << "circularTransects():";
//#endif
for (const auto &p : polygon.outer()) {
snake::Length distance = bg::distance(reference, p) * si::meter;
distances.push_back(distance);
snake::Angle alpha = (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
alpha = alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
angles.push_back(alpha);
//#ifdef DEBUG_CIRCULAR_SURVEY
// qCDebug(CircularGeneratorLog) << "distances, angles,
// coordinates:"; qCDebug(CircularGeneratorLog) <<
// to_string(distance).c_str(); qCDebug(CircularGeneratorLog)
// << to_string(snake::Degree(alpha)).c_str();
// qCDebug(CircularGeneratorLog) << "x = " << p.get<0>() << "y
// = "
// << p.get<1>();
//#endif
}
auto rMin = deltaR; // minimal circle radius
snake::Angle alpha1(0 * degree::degree);
snake::Angle alpha2(360 * degree::degree);
// Determine r_min by successive approximation
if (!bg::within(reference, polygon.outer())) {
rMin = bg::distance(reference, polygon) * si::meter;
}
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 referenceScaled = ClipperLib::IntPoint{
ClipperLib::cInt(std::round(reference.get<0>() * CLIPPER_SCALE)),
ClipperLib::cInt(std::round(reference.get<1>() * CLIPPER_SCALE))};
// Generate circle sectors.
auto rScaled = rMinScaled;
const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
const auto nSectors =
long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
//#ifdef DEBUG_CIRCULAR_SURVEY
// qCDebug(CircularGeneratorLog) << "circularTransects(): sector
// parameres:"; qCDebug(CircularGeneratorLog) << "alpha1: " <<
// to_string(snake::Degree(alpha1)).c_str();
// qCDebug(CircularGeneratorLog) << "alpha2:
// "
// << to_string(snake::Degree(alpha2)).c_str();
// qCDebug(CircularGeneratorLog) << "n: "
// << to_string((alpha2 - alpha1) / deltaAlpha).c_str();
// qCDebug(CircularGeneratorLog)
// << "nSectors: " << nSectors; qCDebug(CircularGeneratorLog) <<
// "rMin: " << to_string(rMin).c_str();
// qCDebug(CircularGeneratorLog)
// << "rMax: " << to_string(rMax).c_str();
// qCDebug(CircularGeneratorLog) << "nTran: " << nTran;
//#endif
using ClipperCircle =
GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
for (auto &sector : sectors) {
ClipperCircle circle(rScaled, referenceScaled);
approximate(circle, nSectors, sector);
rScaled += deltaRScaled;
}
// Clip sectors to polygonENU.
ClipperLib::Path polygonClipper;
snake::FPolygon shrinked;
snake::offsetPolygon(polygon, shrinked, -0.3);
auto &outer = shrinked.outer();
polygonClipper.reserve(outer.size());
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);
// Subtract holes.
if (tiles.size() > 0) {
vector<ClipperLib::Path> processedTiles;
for (const auto &tile : tiles) {
ClipperLib::Path path;
for (const auto &v : tile.outer()) {
path.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)});
}
processedTiles.push_back(std::move(path));
}
clipper.Clear();
for (const auto &child : transectsClipper.Childs) {
clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
}
clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
transectsClipper.Clear();
clipper.Execute(ClipperLib::ctDifference, transectsClipper,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
}
// Extract transects from PolyTree and convert them to
// BoostLineString
for (const auto &child : transectsClipper.Childs) {
snake::FLineString 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::FPoint(x, y));
}
transects.push_back(transect);
}
// Join sectors which where slit due to clipping.
const double th = 0.01;
for (auto ito = transects.begin(); ito < transects.end(); ++ito) {
for (auto iti = ito + 1; iti < transects.end(); ++iti) {
auto dist1 = bg::distance(ito->front(), iti->front());
if (dist1 < th) {
snake::FLineString temp;
for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
temp.push_back(*it);
}
temp.insert(temp.end(), ito->begin(), ito->end());
*ito = temp;
transects.erase(iti);
break;
}
auto dist2 = bg::distance(ito->front(), iti->back());
if (dist2 < th) {
snake::FLineString temp;
temp.insert(temp.end(), iti->begin(), iti->end());
temp.insert(temp.end(), ito->begin(), ito->end());
*ito = temp;
transects.erase(iti);
break;
}
auto dist3 = bg::distance(ito->back(), iti->front());
if (dist3 < th) {
snake::FLineString temp;
temp.insert(temp.end(), ito->begin(), ito->end());
temp.insert(temp.end(), iti->begin(), iti->end());
*ito = temp;
transects.erase(iti);
break;
}
auto dist4 = bg::distance(ito->back(), iti->back());
if (dist4 < th) {
snake::FLineString temp;
temp.insert(temp.end(), ito->begin(), ito->end());
for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
temp.push_back(*it);
}
*ito = temp;
transects.erase(iti);
break;
}
}
}
// Remove short transects
for (auto it = transects.begin(); it < transects.end();) {
if (bg::length(*it) < minLength.value()) {
it = transects.erase(it);
} else {
++it;
}
}
qCDebug(CircularGeneratorLog)
<< "circularTransects(): transect gen. time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - s1)
.count()
<< " ms";
return true;
}
}
return false;
}
} // namespace routing
src/Wima/Snake/CircularGenerator.h deleted 100644 → 0
View file @ 458ab210
#include "GeneratorBase.h"
#include <QGeoCoordinate>
namespace routing {
class CircularGenerator : public GeneratorBase {
Q_OBJECT
public:
CircularGenerator(QObject *parent = nullptr);
CircularGenerator(Data d, QObject *parent = nullptr);
Q_PROPERTY(QGeoCoordinate reference READ reference WRITE setReference NOTIFY
referenceChanged)
Q_PROPERTY(Fact *distance READ distance CONSTANT)
Q_PROPERTY(Fact *deltaAlpha READ deltaAlpha CONSTANT)
Q_PROPERTY(Fact *minLength READ minLength CONSTANT)
virtual QString editorQml() override;
virtual QString mapVisualQml() override;
virtual QString name() override;
virtual QString abbreviation() override;
virtual bool get(Generator &generator) override;
QGeoCoordinate reference() const;
Fact *distance();
Fact *deltaAlpha();
Fact *minLength();
void setReference(const QGeoCoordinate &reference);
static const char *settingsGroup;
static const char *distanceName;
static const char *deltaAlphaName;
static const char *minLengthName;
static const char *refPointLongitudeName;
static const char *refPointLatitudeName;
static const char *refPointAltitudeName;
signals:
void referenceChanged();
public slots:
Q_INVOKABLE void resetReferenceIfInvalid();
Q_INVOKABLE void resetReference();
protected:
virtual void establishConnections() override;
virtual void deleteConnections() override;
private:
bool _connectionsEstablished;
QGeoCoordinate _reference;
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _distance;
SettingsFact _deltaAlpha;
SettingsFact _minLength;
};
} // namespace routing
src/Wima/Snake/GeneratorBase.cc deleted 100644 → 0
View file @ 458ab210
#include "GeneratorBase.h"
namespace routing {
GeneratorBase::GeneratorBase(QObject *parent)
: GeneratorBase(nullptr, parent) {}
GeneratorBase::GeneratorBase(GeneratorBase::Data d, QObject *parent)
: QObject(parent), _d(d) {
establishConnections();
}
GeneratorBase::~GeneratorBase() {}
QString GeneratorBase::editorQml() { return QStringLiteral(""); }
QString GeneratorBase::mapVisualQml() { return QStringLiteral(""); }
GeneratorBase::Data GeneratorBase::data() const { return _d; }
void GeneratorBase::setData(const Data &d) {
deleteConnections();
_d = d;
establishConnections();
}
void GeneratorBase::establishConnections() {}
void GeneratorBase::deleteConnections() {}
} // namespace routing
src/Wima/Snake/GeneratorBase.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QObject>
#include <functional>
#include <memory>
#include "snake.h"
#include "Wima/WimaPlanData.h"
namespace routing {
class GeneratorBase : public QObject {
Q_OBJECT
public:
using Data = std::shared_ptr<AreaData>;
using Generator = std::function<bool(snake::Transects &)>;
explicit GeneratorBase(QObject *parent = nullptr);
explicit GeneratorBase(Data d, QObject *parent = nullptr);
~GeneratorBase();
Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
Q_PROPERTY(QString mapVisualQml READ mapVisualQml CONSTANT)
virtual QString editorQml();
virtual QString mapVisualQml();
virtual QString name() = 0;
virtual QString abbreviation() = 0;
virtual bool get(Generator &generator) = 0;
Data data() const;
void setData(const Data &d);
signals:
void generatorChanged();
protected:
virtual void establishConnections();
virtual void deleteConnections();
Data _d;
};
} // namespace routing
src/Wima/Snake/LinearGenerator.cpp deleted 100644 → 0
View file @ 458ab210
#include "LinearGenerator.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(LinearGeneratorLog, "LinearGeneratorLog")
#define CLIPPER_SCALE 1000000
#include "clipper/clipper.hpp"
#include "SnakeTile.h"
#include "Wima/RoutingThread.h"
namespace routing {
bool linearTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length distance, snake::Angle angle,
snake::Length minLength, snake::Transects &transects);
const char *LinearGenerator::settingsGroup = "LinearGenerator";
const char *LinearGenerator::distanceName = "TransectDistance";
const char *LinearGenerator::alphaName = "Alpha";
const char *LinearGenerator::minLengthName = "MinLength";
LinearGenerator::LinearGenerator(QObject *parent)
: LinearGenerator(nullptr, parent) {}
LinearGenerator::LinearGenerator(GeneratorBase::Data d, QObject *parent)
: GeneratorBase(d, parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/LinearGenerator.SettingsGroup.json"), this)),
_distance(settingsGroup, _metaDataMap[distanceName]),
_alpha(settingsGroup, _metaDataMap[alphaName]),
_minLength(settingsGroup, _metaDataMap[minLengthName]) {
establishConnections();
}
QString LinearGenerator::editorQml() {
return QStringLiteral("LinearGeneratorEditor.qml");
}
QString LinearGenerator::name() { return QStringLiteral("Linear Generator"); }
QString LinearGenerator::abbreviation() { return QStringLiteral("L. Gen."); }
bool LinearGenerator::get(Generator &generator) {
if (_d) {
if (this->_d->isValid()) {
// Prepare data.
auto origin = this->_d->origin();
origin.setAltitude(0);
if (!origin.isValid()) {
qCDebug(LinearGeneratorLog) << "get(): origin invalid." << origin;
}
auto geoPolygon = this->_d->measurementArea().coordinateList();
for (auto &v : geoPolygon) {
if (v.isValid()) {
v.setAltitude(0);
} else {
qCDebug(LinearGeneratorLog) << "get(): measurement area invalid.";
for (const auto &w : geoPolygon) {
qCDebug(LinearGeneratorLog) << w;
}
return false;
}
}
auto pPolygon = std::make_shared<snake::FPolygon>();
snake::areaToEnu(origin, geoPolygon, *pPolygon);
// Progress and tiles.
const auto &progress = this->_d->measurementArea().progress();
const auto *tiles = this->_d->measurementArea().tiles();
auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
const QObject *obj = (*tiles)[int(i)];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
pTiles->push_back(std::move(tileENU));
} else {
qCDebug(LinearGeneratorLog) << "get(): tile == nullptr";
return false;
}
}
}
} else {
qCDebug(LinearGeneratorLog)
<< "get(): progress.size() != tiles->count().";
return false;
}
auto geoDepot = this->_d->serviceArea().depot();
if (!geoDepot.isValid()) {
qCDebug(LinearGeneratorLog) << "get(): depot invalid." << geoDepot;
return false;
}
snake::FPoint depot;
snake::toENU(origin, geoDepot, depot);
// Fetch transect parameter.
auto distance =
snake::Length(this->_distance.rawValue().toDouble() * bu::si::meter);
auto minLength =
snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
auto alpha =
snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);
generator = [depot, pPolygon, pTiles, distance, alpha,
minLength](snake::Transects &transects) -> bool {
bool value = linearTransects(*pPolygon, *pTiles, distance, alpha,
minLength, transects);
transects.insert(transects.begin(), snake::FLineString{depot});
return value;
};
return true;
} else {
qCDebug(LinearGeneratorLog) << "get(): data invalid.";
return false;
}
} else {
qCDebug(LinearGeneratorLog) << "get(): data member not set.";
return false;
}
}
Fact *LinearGenerator::distance() { return &_distance; }
Fact *LinearGenerator::alpha() { return &_alpha; }
Fact *LinearGenerator::minLength() { return &_minLength; }
void LinearGenerator::establishConnections() {
if (this->_d && !this->_connectionsEstablished) {
connect(this->_d.get(), &AreaData::originChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::progressChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::tileDataChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(), &WimaMeasurementAreaData::pathChanged,
this, &GeneratorBase::generatorChanged);
connect(&this->_d->serviceArea(), &WimaServiceAreaData::depotChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->joinedArea(), &WimaJoinedAreaData::pathChanged, this,
&GeneratorBase::generatorChanged);
connect(this->distance(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
connect(this->alpha(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
connect(this->minLength(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
this->_connectionsEstablished = true;
}
}
void LinearGenerator::deleteConnections() {
if (this->_d && this->_connectionsEstablished) {
connect(this->_d.get(), &AreaData::originChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::progressChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(),
&WimaMeasurementAreaData::tileDataChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->measurementArea(), &WimaMeasurementAreaData::pathChanged,
this, &GeneratorBase::generatorChanged);
connect(&this->_d->serviceArea(), &WimaServiceAreaData::depotChanged, this,
&GeneratorBase::generatorChanged);
connect(&this->_d->joinedArea(), &WimaJoinedAreaData::pathChanged, this,
&GeneratorBase::generatorChanged);
connect(this->distance(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
connect(this->alpha(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
connect(this->minLength(), &Fact::rawValueChanged, this,
&GeneratorBase::generatorChanged);
this->_connectionsEstablished = false;
}
}
bool linearTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length distance, snake::Angle angle,
snake::Length minLength, snake::Transects &transects) {
namespace tr = bg::strategy::transform;
auto s1 = std::chrono::high_resolution_clock::now();
// Check preconitions
if (polygon.outer().size() >= 3) {
// Convert to ENU system.
std::string error;
// Check validity.
if (!bg::is_valid(polygon, error)) {
std::stringstream ss;
ss << bg::wkt(polygon);
qCDebug(LinearGeneratorLog) << "linearTransects(): "
"invalid polygon. "
<< error.c_str() << ss.str().c_str();
} else {
tr::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
180 / M_PI);
// Rotate polygon by angle and calculate bounding box.
snake::FPolygon polygonENURotated;
bg::transform(polygon.outer(), polygonENURotated.outer(), rotate);
snake::FBox box;
boost::geometry::envelope(polygonENURotated, box);
double x0 = box.min_corner().get<0>();
double y0 = box.min_corner().get<1>();
double x1 = box.max_corner().get<0>();
double y1 = box.max_corner().get<1>();
// Generate transects and convert them to clipper path.
size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects
vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i = 0; i < num_t; ++i) {
// calculate transect
snake::FPoint v1{x0, y0 + i * distance.value()};
snake::FPoint v2{x1, y0 + i * distance.value()};
snake::FLineString transect;
transect.push_back(v1);
transect.push_back(v2);
// transform back
snake::FLineString temp_transect;
tr::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
-angle.value() * 180 / M_PI);
bg::transform(transect, temp_transect, rotate_back);
// to clipper
ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
temp_transect[0].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(
temp_transect[0].get<1>() * CLIPPER_SCALE)};
ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
temp_transect[1].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(
temp_transect[1].get<1>() * CLIPPER_SCALE)};
ClipperLib::Path path{c1, c2};
transectsClipper.push_back(path);
}
if (transectsClipper.size() == 0) {
std::stringstream ss;
ss << "Not able to generate transects. Parameter: distance = "
<< distance << std::endl;
qCDebug(LinearGeneratorLog)
<< "linearTransects(): " << ss.str().c_str();
return false;
}
// Convert measurement area to clipper path.
snake::FPolygon shrinked;
snake::offsetPolygon(polygon, shrinked, -0.2);
auto &outer = shrinked.outer();
ClipperLib::Path polygonClipper;
for (auto vertex : outer) {
polygonClipper.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib::Clipper clipper;
clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
ClipperLib::PolyTree clippedTransecs;
clipper.Execute(ClipperLib::ctIntersection, clippedTransecs,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Subtract holes.
if (tiles.size() > 0) {
vector<ClipperLib::Path> processedTiles;
for (const auto &tile : tiles) {
ClipperLib::Path path;
for (const auto &v : tile.outer()) {
path.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)});
}
processedTiles.push_back(std::move(path));
}
clipper.Clear();
for (const auto &child : clippedTransecs.Childs) {
clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
}
clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
clippedTransecs.Clear();
clipper.Execute(ClipperLib::ctDifference, clippedTransecs,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
}
// Extract transects from PolyTree and convert them to BoostLineString
for (const auto &child : clippedTransecs.Childs) {
const auto &clipperTransect = child->Contour;
snake::FPoint v1{
static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
snake::FPoint v2{
static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};
snake::FLineString transect{v1, v2};
if (bg::length(transect) >= minLength.value()) {
transects.push_back(transect);
}
}
if (transects.size() == 0) {
std::stringstream ss;
ss << "Not able to generatetransects. Parameter: minLength = "
<< minLength << std::endl;
qCDebug(LinearGeneratorLog)
<< "linearTransects(): " << ss.str().c_str();
return false;
}
qCDebug(LinearGeneratorLog)
<< "linearTransects(): time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - s1)
.count()
<< " ms";
return true;
}
}
return false;
}
} // namespace routing
src/Wima/Snake/LinearGenerator.h deleted 100644 → 0
View file @ 458ab210
#include "GeneratorBase.h"
#include <QGeoCoordinate>
namespace routing {
class LinearGenerator : public GeneratorBase {
Q_OBJECT
public:
LinearGenerator(QObject *parent = nullptr);
LinearGenerator(Data d, QObject *parent = nullptr);
Q_PROPERTY(Fact *distance READ distance CONSTANT)
Q_PROPERTY(Fact *alpha READ alpha CONSTANT)
Q_PROPERTY(Fact *minLength READ minLength CONSTANT)
virtual QString editorQml() override;
virtual QString name() override;
virtual QString abbreviation() override;
virtual bool get(Generator &generator) override;
Fact *distance();
Fact *alpha();
Fact *minLength();
static const char *settingsGroup;
static const char *distanceName;
static const char *alphaName;
static const char *minLengthName;
protected:
virtual void establishConnections() override;
virtual void deleteConnections() override;
private:
bool _connectionsEstablished;
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _distance;
SettingsFact _alpha;
SettingsFact _minLength;
};
} // namespace routing
src/Wima/Snake/NemoInterface.cpp deleted 100644 → 0
View file @ 458ab210
#include "NemoInterface.h"
#include "SnakeTilesLocal.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"
#include <shared_mutex>
#include <QTimer>
#include "QNemoHeartbeat.h"
#include "QNemoProgress.h"
#include "Wima/Geometry/WimaMeasurementArea.h"
#include "Wima/Snake/SnakeTile.h"
#include "Wima/Snake/snake.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
#define EVENT_TIMER_INTERVAL 100 // ms
auto static timeoutInterval = std::chrono::milliseconds(3000);
using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
using UniqueLock = std::unique_lock<std::shared_timed_mutex>;
using SharedLock = std::shared_lock<std::shared_timed_mutex>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
class NemoInterface::Impl {
using TimePoint = std::chrono::time_point<std::chrono::high_resolution_clock>;
public:
Impl(NemoInterface *p);
void start();
void stop();
void setTileData(const TileData &tileData);
bool hasTileData(const TileData &tileData) const;
void setAutoPublish(bool ap);
void setHoldProgress(bool hp);
void publishTileData();
NemoInterface::STATUS status();
QVector<int> progress();
bool running();
private:
void doTopicServiceSetup();
void loop();
static STATUS heartbeatToStatus(
const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb);
//!
//! \brief Publishes tilesENU
//! \pre this->tilesENUMutex must be locked
//!
void publishTilesENU();
//!
//! \brief Publishes ENUOrigin
//! \pre this->ENUOriginMutex must be locked
//!
void publishENUOrigin();
bool setStatus(NemoInterface::STATUS s);
// Data.
SnakeTilesLocal tilesENU;
mutable std::shared_timed_mutex tilesENUMutex;
QGeoCoordinate ENUOrigin;
mutable std::shared_timed_mutex ENUOriginMutex;
QNemoProgress qProgress;
mutable std::shared_timed_mutex progressMutex;
TimePoint nextTimeout;
mutable std::shared_timed_mutex timeoutMutex;
std::atomic<NemoInterface::STATUS> status_;
// Not protected data.
TileData tileData;
// Internals
std::atomic_bool running_;
std::atomic_bool topicServiceSetupDone;
ROSBridgePtr pRosBridge;
QTimer loopTimer;
NemoInterface *parent;
};
using StatusMap = std::map<NemoInterface::STATUS, QString>;
static StatusMap statusMap{
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::HEARTBEAT_DETECTED, "Heartbeat Detected"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::TIMEOUT, "Timeout"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::INVALID_HEARTBEAT, "Error"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};
NemoInterface::Impl::Impl(NemoInterface *p)
: nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED),
running_(false), topicServiceSetupDone(false), parent(p) {
// ROS Bridge.
WimaSettings *wimaSettings =
qgcApp()->toolbox()->settingsManager()->wimaSettings();
auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
auto setConnectionString = [connectionStringFact, this] {
auto connectionString = connectionStringFact->rawValue().toString();
if (ros_bridge::isValidConnectionString(
connectionString.toLocal8Bit().data())) {
} else {
qgcApp()->warningMessageBoxOnMainThread(
"Nemo Interface",
"Websocket connection string possibly invalid: " + connectionString +
". Trying to connect anyways.");
}
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
setConnectionString();
// Periodic.
connect(&this->loopTimer, &QTimer::timeout, [this] { this->loop(); });
this->loopTimer.start(EVENT_TIMER_INTERVAL);
}
void NemoInterface::Impl::start() {
this->running_ = true;
emit this->parent->runningChanged();
}
void NemoInterface::Impl::stop() {
this->running_ = false;
emit this->parent->runningChanged();
}
void NemoInterface::Impl::setTileData(const TileData &tileData) {
this->tileData = tileData;
if (tileData.tiles.count() > 0) {
std::lock(this->ENUOriginMutex, this->tilesENUMutex);
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
const auto *obj = tileData.tiles[0];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
if (tile->coordinateList().size() > 0) {
if (tile->coordinateList().first().isValid()) {
this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles[i];
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
break;
}
}
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
}
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
}
}
} else {
this->tileData.clear();
std::lock(this->ENUOriginMutex, this->tilesENUMutex);
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
this->ENUOrigin = QGeoCoordinate(0, 0, 0);
this->tilesENU = SnakeTilesLocal();
}
}
bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
return this->tileData == tileData;
}
void NemoInterface::Impl::publishTileData() {
std::lock(this->ENUOriginMutex, this->tilesENUMutex);
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
if (this->tilesENU.polygons().size() > 0 && this->running_ &&
this->topicServiceSetupDone) {
this->publishENUOrigin();
this->publishTilesENU();
}
}
NemoInterface::STATUS NemoInterface::Impl::status() { return status_.load(); }
QVector<int> NemoInterface::Impl::progress() {
SharedLock lk(this->progressMutex);
return this->qProgress.progress();
}
bool NemoInterface::Impl::running() { return this->running_.load(); }
void NemoInterface::Impl::doTopicServiceSetup() {
using namespace ros_bridge::messages;
// snake tiles.
{
SharedLock lk(this->tilesENUMutex);
this->pRosBridge->advertiseTopic(
"/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
}
// snake origin.
{
SharedLock lk(this->ENUOriginMutex);
this->pRosBridge->advertiseTopic(
"/snake/origin", geographic_msgs::geo_point::messageType().c_str());
}
// Subscribe nemo progress.
this->pRosBridge->subscribe(
"/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc) {
std::lock(this->progressMutex, this->tilesENUMutex,
this->ENUOriginMutex);
UniqueLock lk1(this->progressMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock);
int requiredSize = this->tilesENU.polygons().size();
auto &progressMsg = this->qProgress;
if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
progressMsg.progress().size() !=
requiredSize) { // Some error occured.
progressMsg.progress().clear();
qgcApp()->informationMessageBoxOnMainThread(
"Nemo Interface", "Invalid progress message received.");
}
emit this->parent->progressChanged();
lk1.unlock();
lk2.unlock();
lk3.unlock();
});
// Subscribe /nemo/heartbeat.
this->pRosBridge->subscribe(
"/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc) {
// auto start = std::chrono::high_resolution_clock::now();
nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
this->setStatus(STATUS::INVALID_HEARTBEAT);
} else {
this->setStatus(heartbeatToStatus(heartbeatMsg));
}
if (this->status_ == STATUS::INVALID_HEARTBEAT) {
UniqueLock lk(this->timeoutMutex);
this->nextTimeout = TimePoint::max();
} else if (this->status_ == STATUS::HEARTBEAT_DETECTED) {
UniqueLock lk(this->timeoutMutex);
this->nextTimeout =
std::chrono::high_resolution_clock::now() + timeoutInterval;
}
// auto delta =
// std::chrono::duration_cast<std::chrono::milliseconds>(
// std::chrono::high_resolution_clock::now() - start);
// std::cout << "/nemo/heartbeat callback time: " <<
// delta.count() << " ms"
// << std::endl;
});
// Advertise /snake/get_origin.
this->pRosBridge->advertiseService(
"/snake/get_origin", "snake_msgs/GetOrigin",
[this](JsonDocUPtr) -> JsonDocUPtr {
using namespace ros_bridge::messages;
SharedLock lk(this->ENUOriginMutex);
JsonDocUPtr pDoc(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
auto &origin = this->ENUOrigin;
rapidjson::Value jOrigin(rapidjson::kObjectType);
lk.unlock();
if (geographic_msgs::geo_point::toJson(origin, jOrigin,
pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
<< "/snake/get_origin service: could not create json document.";
}
return pDoc;
});
// Advertise /snake/get_tiles.
this->pRosBridge->advertiseService(
"/snake/get_tiles", "snake_msgs/GetTiles",
[this](JsonDocUPtr) -> JsonDocUPtr {
SharedLock lk(this->tilesENUMutex);
JsonDocUPtr pDoc(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
<< "/snake/get_tiles service: could not create json document.";
}
return pDoc;
});
}
void NemoInterface::Impl::loop() {
// Check ROS Bridge status and do setup if necessary.
if (this->running_) {
if (!this->pRosBridge->isRunning()) {
this->pRosBridge->start();
this->loop();
} else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
!this->topicServiceSetupDone) {
this->doTopicServiceSetup();
this->topicServiceSetupDone = true;
this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else if (this->pRosBridge->isRunning() &&
!this->pRosBridge->connected() && this->topicServiceSetupDone) {
this->pRosBridge->reset();
this->pRosBridge->start();
this->topicServiceSetupDone = false;
this->setStatus(STATUS::TIMEOUT);
}
} else if (this->pRosBridge->isRunning()) {
this->pRosBridge->reset();
this->topicServiceSetupDone = false;
}
// Check if heartbeat timeout occured.
if (this->running_ && this->topicServiceSetupDone) {
UniqueLock lk(this->timeoutMutex);
if (this->nextTimeout != TimePoint::max() &&
this->nextTimeout < std::chrono::high_resolution_clock::now()) {
lk.unlock();
if (this->pRosBridge->isRunning() && this->pRosBridge->connected()) {
this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else {
this->setStatus(STATUS::TIMEOUT);
}
}
}
}
NemoInterface::STATUS NemoInterface::Impl::heartbeatToStatus(
const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb) {
if (STATUS(hb.status()) == STATUS::HEARTBEAT_DETECTED)
return STATUS::HEARTBEAT_DETECTED;
else
return STATUS::INVALID_HEARTBEAT;
}
void NemoInterface::Impl::publishTilesENU() {
using namespace ros_bridge::messages;
JsonDocUPtr jSnakeTiles(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) {
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
} else {
qCWarning(NemoInterfaceLog)
<< "Impl::publishTilesENU: could not create json document.";
}
}
void NemoInterface::Impl::publishENUOrigin() {
using namespace ros_bridge::messages;
JsonDocUPtr jOrigin(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
jOrigin->GetAllocator())) {
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
} else {
qCWarning(NemoInterfaceLog)
<< "Impl::publishENUOrigin: could not create json document.";
}
}
bool NemoInterface::Impl::setStatus(NemoInterface::STATUS s) {
if (s != this->status_) {
this->status_ = s;
emit this->parent->statusChanged();
return true;
} else {
return false;
}
}
// ===============================================================
// NemoInterface
NemoInterface::NemoInterface(QObject *parent)
: QObject(parent), pImpl(std::make_unique<NemoInterface::Impl>(this)) {}
NemoInterface::~NemoInterface() {}
void NemoInterface::start() { this->pImpl->start(); }
void NemoInterface::stop() { this->pImpl->stop(); }
void NemoInterface::publishTileData() { this->pImpl->publishTileData(); }
void NemoInterface::requestProgress() {
qCWarning(NemoInterfaceLog) << "requestProgress(): dummy.";
}
void NemoInterface::setTileData(const TileData &tileData) {
this->pImpl->setTileData(tileData);
}
bool NemoInterface::hasTileData(const TileData &tileData) const {
return this->pImpl->hasTileData(tileData);
}
int NemoInterface::status() const { return integral(this->pImpl->status()); }
NemoInterface::STATUS NemoInterface::statusEnum() const {
return this->pImpl->status();
}
QString NemoInterface::statusString() const {
return statusMap.at(this->pImpl->status());
}
QVector<int> NemoInterface::progress() const { return this->pImpl->progress(); }
QString NemoInterface::editorQml() {
return QStringLiteral("NemoInterface.qml");
}
bool NemoInterface::running() { return this->pImpl->running(); }
src/Wima/Snake/NemoInterface.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QGeoCoordinate>
#include <QObject>
#include <memory>
class TileData;
class NemoInterface : public QObject {
Q_OBJECT
class Impl;
using PImpl = std::unique_ptr<Impl>;
public:
enum class STATUS {
NOT_CONNECTED = 0,
HEARTBEAT_DETECTED = 1,
WEBSOCKET_DETECTED = 2,
TIMEOUT = -1,
INVALID_HEARTBEAT = -2
};
explicit NemoInterface(QObject *parent = nullptr);
~NemoInterface() override;
Q_PROPERTY(int status READ status NOTIFY statusChanged)
Q_PROPERTY(QString statusString READ statusString NOTIFY statusChanged)
Q_PROPERTY(QVector<int> progress READ progress NOTIFY progressChanged)
Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
Q_PROPERTY(bool running READ running NOTIFY runningChanged)
Q_INVOKABLE void start();
Q_INVOKABLE void stop();
Q_INVOKABLE void publishTileData();
Q_INVOKABLE void requestProgress();
void setTileData(const TileData &tileData);
bool hasTileData(const TileData &tileData) const;
void setAutoPublish(bool ap);
void setHoldProgress(bool hp);
int status() const;
STATUS statusEnum() const;
QString statusString() const;
QVector<int> progress() const;
QString editorQml();
bool running();
signals:
void statusChanged();
void progressChanged();
void runningChanged();
private:
PImpl pImpl;
};
src/Wima/Snake/QNemoHeartbeat.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
using QNemoHeartbeat = ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat;
src/Wima/Snake/QNemoProgress.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QVector>
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
namespace nemo = ros_bridge::messages::nemo_msgs;
typedef nemo::progress::GenericProgress<int, QVector> QNemoProgress;
src/Wima/Snake/SnakeThread.cc deleted 100644 → 0
View file @ 458ab210
#include "SnakeThread.h"
#include <QGeoCoordinate>
#include <QMutexLocker>
#include "QGCApplication.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"
#include <iostream>
#include <memory>
#include <mutex>
#include <shared_mutex>
#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "snake.h"
template <class Callable> class CommandRAII {
public:
CommandRAII(Callable &fun) : _fun(fun) {}
~CommandRAII() { _fun(); }
private:
Callable _fun;
};
using QVariantList = QList<QVariant>;
using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;
class SnakeThread::Impl {
public:
struct Input {
Input()
: tileWidth(5 * si::meter), tileHeight(5 * si::meter),
minTileArea(5 * si::meter * si::meter), lineDistance(2 * si::meter),
minTransectLength(1 * si::meter), scenarioChanged(true),
progressChanged(true), routeParametersChanged(true) {}
QList<QGeoCoordinate> mArea;
QGeoCoordinate ENUOrigin;
QList<QGeoCoordinate> sArea;
QList<QGeoCoordinate> corridor;
Length tileWidth;
Length tileHeight;
Area minTileArea;
Length lineDistance;
Length minTransectLength;
QNemoProgress progress;
std::atomic_bool scenarioChanged;
std::atomic_bool progressChanged;
std::atomic_bool routeParametersChanged;
mutable std::shared_timed_mutex mutex;
};
struct Output {
Output()
: calcInProgress(false), tilesUpdated(false), waypointsUpdated(false),
progressUpdated(false) {}
SnakeTiles tiles;
QVariantList tileCenterPoints;
SnakeTilesLocal tilesENU;
QVector<QPointF> tileCenterPointsENU;
QGeoCoordinate ENUOrigin;
QVector<QGeoCoordinate> waypoints;
QVector<QGeoCoordinate> arrivalPath;
QVector<QGeoCoordinate> returnPath;
QVector<QPointF> waypointsENU;
QVector<QPointF> arrivalPathENU;
QVector<QPointF> returnPathENU;
QString errorMessage;
std::atomic_bool calcInProgress;
bool tilesUpdated;
bool waypointsUpdated;
bool progressUpdated;
mutable std::shared_timed_mutex mutex;
};
Impl(SnakeThread *p);
bool precondition() const;
bool doTopicServiceSetup();
// Internal data.
snake::BoostPolygon mAreaENU;
snake::BoostPolygon sAreaENU;
snake::BoostPolygon corridorENU;
snake::BoostPolygon jAreaENU;
std::vector<snake::BoostPolygon> tilesENU;
QGeoCoordinate ENUOrigin;
SnakeThread *parent;
// Input
Input input;
// Output
Output output;
};
SnakeThread::Impl::Impl(SnakeThread *p) : parent(p) {}
SnakeThread::SnakeThread(QObject *parent)
: QThread(parent), pImpl(std::make_unique<Impl>(this)) {}
SnakeThread::~SnakeThread() {}
void SnakeThread::setMeasurementArea(
const QList<QGeoCoordinate> &measurementArea) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.mArea = measurementArea;
for (auto &vertex : this->pImpl->input.mArea) {
vertex.setAltitude(0);
}
}
void SnakeThread::setServiceArea(const QList<QGeoCoordinate> &serviceArea) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.sArea = serviceArea;
for (auto &vertex : this->pImpl->input.sArea) {
vertex.setAltitude(0);
}
}
void SnakeThread::setCorridor(const QList<QGeoCoordinate> &corridor) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.corridor = corridor;
for (auto &vertex : this->pImpl->input.corridor) {
vertex.setAltitude(0);
}
}
void SnakeThread::setProgress(const QVector<int> &progress) {
this->pImpl->input.progressChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.progress.progress() = progress;
}
SnakeTiles SnakeThread::tiles() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.tiles;
}
SnakeTilesLocal SnakeThread::tilesENU() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.tilesENU;
}
QGeoCoordinate SnakeThread::ENUOrigin() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.ENUOrigin;
}
QVariantList SnakeThread::tileCenterPoints() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.tileCenterPoints;
}
QVector<int> SnakeThread::progress() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->input.progress.progress();
}
bool SnakeThread::calcInProgress() const {
return this->pImpl->output.calcInProgress.load();
}
QString SnakeThread::errorMessage() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.errorMessage;
}
bool SnakeThread::tilesUpdated() {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.tilesUpdated;
}
bool SnakeThread::waypointsUpdated() {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.waypointsUpdated;
}
bool SnakeThread::progressUpdated() {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.progressUpdated;
}
QVector<QGeoCoordinate> SnakeThread::waypoints() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.waypoints;
}
QVector<QGeoCoordinate> SnakeThread::arrivalPath() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.arrivalPath;
}
QVector<QGeoCoordinate> SnakeThread::returnPath() const {
SharedLock lk(this->pImpl->output.mutex);
return this->pImpl->output.returnPath;
}
Length SnakeThread::lineDistance() const {
SharedLock lk(this->pImpl->input.mutex);
return this->pImpl->input.lineDistance;
}
void SnakeThread::setLineDistance(Length lineDistance) {
this->pImpl->input.routeParametersChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.routeParametersChanged = true;
this->pImpl->input.lineDistance = lineDistance;
}
Area SnakeThread::minTileArea() const {
SharedLock lk(this->pImpl->input.mutex);
return this->pImpl->input.minTileArea;
}
void SnakeThread::setMinTileArea(Area minTileArea) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.minTileArea = minTileArea;
}
Length SnakeThread::tileHeight() const {
SharedLock lk(this->pImpl->input.mutex);
return this->pImpl->input.tileHeight;
}
void SnakeThread::setTileHeight(Length tileHeight) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.tileHeight = tileHeight;
}
Length SnakeThread::tileWidth() const {
SharedLock lk(this->pImpl->input.mutex);
return this->pImpl->input.tileWidth;
}
void SnakeThread::setTileWidth(Length tileWidth) {
this->pImpl->input.scenarioChanged = true;
UniqueLock lk(this->pImpl->input.mutex);
this->pImpl->input.tileWidth = tileWidth;
}
void SnakeThread::run() {
#ifndef NDEBUG
auto startTime = std::chrono::high_resolution_clock::now();
#endif
// Signal that calculation is in progress.
this->pImpl->output.calcInProgress.store(true);
emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
#ifndef NDEBUG
auto onExit = [this, &startTime] {
#else
auto onExit = [this] {
#endif
#ifndef NDEBUG
qDebug() << "SnakeThread::run() execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - startTime)
.count()
<< " ms";
#endif
this->pImpl->output.calcInProgress.store(false);
emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
};
CommandRAII<decltype(onExit)> commandRAII(onExit);
bool tilesValid = true;
bool progressValid = false;
snake::Length lineDistance;
snake::Length minTransectLength;
std::vector<int> progress;
{
// Check preconditions.
SharedLock lk(this->pImpl->input.mutex);
if (this->pImpl->input.mArea.size() < 3) {
UniqueLock uLock(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = "Measurement area invalid: size < 3.";
tilesValid = false;
} else if (this->pImpl->input.sArea.size() < 3) {
UniqueLock uLock(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = "Service area invalid: size < 3.";
tilesValid = false;
} else {
// Update Scenario if necessary
if (this->pImpl->input.scenarioChanged) {
// Set Origin
this->pImpl->ENUOrigin = this->pImpl->input.mArea.front();
// Update measurement area.
this->pImpl->mAreaENU.clear();
snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.mArea,
this->pImpl->mAreaENU);
// Update service area.
this->pImpl->sAreaENU.clear();
snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.sArea,
this->pImpl->sAreaENU);
// Update corridor.
this->pImpl->corridorENU.clear();
snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.corridor,
this->pImpl->corridorENU);
// Fetch parametes.
auto tileHeight = this->pImpl->input.tileHeight;
auto tileWidth = this->pImpl->input.tileWidth;
auto minTileArea = this->pImpl->input.minTileArea;
// Update tiles.
this->pImpl->tilesENU.clear();
this->pImpl->jAreaENU.clear();
std::string errorString;
snake::BoundingBox bbox;
if (!snake::joinedArea(this->pImpl->mAreaENU, this->pImpl->sAreaENU,
this->pImpl->corridorENU, this->pImpl->jAreaENU,
errorString) ||
!snake::tiles(this->pImpl->mAreaENU, tileHeight, tileWidth,
minTileArea, this->pImpl->tilesENU, bbox,
errorString)) {
UniqueLock uLock(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = errorString.c_str();
tilesValid = false;
}
}
if (tilesValid) {
// Sample data
lineDistance = this->pImpl->input.lineDistance;
minTransectLength = this->pImpl->input.minTransectLength;
// Verify progress.
size_t nTiles = this->pImpl->tilesENU.size();
if (size_t(this->pImpl->input.progress.progress().size()) != nTiles) {
for (size_t i = 0; i < nTiles; ++i) {
progress.push_back(0);
}
} else {
for (size_t i = 0; i < nTiles; ++i) {
progress.push_back(this->pImpl->input.progress.progress()[i]);
}
}
progressValid = tilesValid;
}
}
}
bool waypointsValid = true;
snake::Transects transects;
std::vector<snake::TransectInfo> transectsInfo;
snake::Route route;
bool needWaypointUpdate = this->pImpl->input.scenarioChanged ||
this->pImpl->input.routeParametersChanged ||
this->pImpl->input.progressChanged;
if (tilesValid) {
if (needWaypointUpdate) {
// Data needed for transects.
std::string errorString;
snake::BoundingBox bbox;
snake::minimalBoundingBox(this->pImpl->mAreaENU, bbox);
snake::Angle alpha(bbox.angle * si::radian);
snake::BoostPoint home;
snake::polygonCenter(this->pImpl->sAreaENU, home);
transects.push_back(bg::model::linestring<snake::BoostPoint>{home});
// Create transects.
bool success = snake::transectsFromScenario(
lineDistance, minTransectLength, alpha, this->pImpl->mAreaENU,
this->pImpl->tilesENU, progress, transects, errorString);
if (!success) {
UniqueLock lk(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = errorString.c_str();
waypointsValid = false;
} else if (transects.size() > 1) {
// Route transects.
success = snake::route(this->pImpl->jAreaENU, transects, transectsInfo,
route, errorString);
if (!success) {
UniqueLock lk(this->pImpl->output.mutex);
this->pImpl->output.errorMessage = errorString.c_str();
waypointsValid = false;
}
} else {
// No transects
waypointsValid = false;
}
}
} else {
waypointsValid = false;
}
UniqueLock lk(this->pImpl->output.mutex);
// Store tiles.
this->pImpl->output.tilesUpdated = false;
if (!tilesValid) {
// Clear some output data.
this->pImpl->output.tiles.polygons().clear();
this->pImpl->output.tileCenterPoints.clear();
this->pImpl->output.tilesENU.polygons().clear();
this->pImpl->output.tileCenterPointsENU.clear();
this->pImpl->output.ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
this->pImpl->output.tilesUpdated = true;
} else if (this->pImpl->input.scenarioChanged) {
this->pImpl->input.scenarioChanged = false;
// Clear some output data.
this->pImpl->output.tiles.polygons().clear();
this->pImpl->output.tileCenterPoints.clear();
this->pImpl->output.tilesENU.polygons().clear();
this->pImpl->output.tileCenterPointsENU.clear();
// Convert and store scenario data.
const auto &tiles = this->pImpl->tilesENU;
for (unsigned int i = 0; i < tiles.size(); ++i) {
const auto &tile = tiles[i];
SnakeTile geoTile;
SnakeTileLocal enuTile;
for (size_t i = 0; i < tile.outer().size() - 1; ++i) {
const auto &p = tile.outer()[i];
QPointF enuVertex{p.get<0>(), p.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(this->pImpl->ENUOrigin, p, geoVertex);
enuTile.polygon().points().push_back(enuVertex);
geoTile.push_back(geoVertex);
}
snake::BoostPoint centerPoint;
snake::polygonCenter(tile, centerPoint);
QPointF enuVertex(centerPoint.get<0>(), centerPoint.get<1>());
QGeoCoordinate geoVertex;
snake::fromENU(this->pImpl->ENUOrigin, centerPoint, geoVertex);
geoTile.setCenter(geoVertex);
this->pImpl->output.tiles.polygons().push_back(geoTile);
this->pImpl->output.tileCenterPoints.push_back(
QVariant::fromValue(geoVertex));
this->pImpl->output.tilesENU.polygons().push_back(enuTile);
this->pImpl->output.tileCenterPointsENU.push_back(enuVertex);
}
this->pImpl->output.tilesUpdated = true;
}
// Store progress.
this->pImpl->output.progressUpdated = false;
if (!progressValid) {
this->pImpl->input.progress.progress().clear();
this->pImpl->output.progressUpdated = true;
} else if (this->pImpl->input.progressChanged) {
if (progress.size() == this->pImpl->tilesENU.size()) {
auto &qProgress = this->pImpl->input.progress;
qProgress.progress().clear();
for (const auto &p : progress) {
qProgress.progress().push_back(p);
}
}
this->pImpl->output.progressUpdated = true;
}
// Store waypoints.
if (!waypointsValid) {
// Clear waypoints.
this->pImpl->output.arrivalPath.clear();
this->pImpl->output.returnPath.clear();
this->pImpl->output.arrivalPathENU.clear();
this->pImpl->output.returnPathENU.clear();
this->pImpl->output.waypoints.clear();
this->pImpl->output.waypointsENU.clear();
this->pImpl->output.waypointsUpdated = true;
} else if (needWaypointUpdate) {
// Clear waypoints.
this->pImpl->output.arrivalPath.clear();
this->pImpl->output.returnPath.clear();
this->pImpl->output.arrivalPathENU.clear();
this->pImpl->output.returnPathENU.clear();
this->pImpl->output.waypoints.clear();
this->pImpl->output.waypointsENU.clear();
// Store arrival path.
const auto &info = transectsInfo.front();
const auto &firstTransect = transects[info.index];
const auto &firstWaypoint =
info.reversed ? firstTransect.front() : firstTransect.back();
long startIdx = 0;
for (long i = 0; i < long(route.size()); ++i) {
const auto &boostVertex = route[i];
if (boostVertex == firstWaypoint) {
startIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(this->pImpl->ENUOrigin, boostVertex, geoVertex);
this->pImpl->output.arrivalPathENU.push_back(enuVertex);
this->pImpl->output.arrivalPath.push_back(geoVertex);
}
// Store return path.
long endIdx = 0;
const auto &info2 = transectsInfo.back();
const auto &lastTransect = transects[info2.index];
const auto &lastWaypoint =
info2.reversed ? lastTransect.front() : lastTransect.back();
for (long i = route.size() - 1; i >= 0; --i) {
const auto &boostVertex = route[i];
if (boostVertex == lastWaypoint) {
endIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(this->pImpl->ENUOrigin, boostVertex, geoVertex);
this->pImpl->output.returnPathENU.push_back(enuVertex);
this->pImpl->output.returnPath.push_back(geoVertex);
}
// Store waypoints.
for (long i = startIdx; i <= endIdx; ++i) {
const auto &boostVertex = route[i];
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(this->pImpl->ENUOrigin, boostVertex, geoVertex);
this->pImpl->output.waypointsENU.push_back(enuVertex);
this->pImpl->output.waypoints.push_back(geoVertex);
}
// Store waypoints.
// for (const auto &t : transects) {
// for (const auto &v : t) {
// QPointF enuVertex{v.get<0>(), v.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, v, geoVertex);
// this->pImpl->output.waypointsENU.push_back(enuVertex);
// this->pImpl->output.waypoints.push_back(geoVertex);
// }
// }
this->pImpl->output.waypointsUpdated = true;
}
}
src/Wima/Snake/SnakeThread.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QGeoCoordinate>
#include <QList>
#include <QObject>
#include <QThread>
#include "QNemoProgress.h"
#include "SnakeTiles.h"
#include "SnakeTilesLocal.h"
#include <boost/units/systems/si.hpp>
using namespace boost::units;
using Length = quantity<si::length>;
using Area = quantity<si::area>;
class SnakeThread : public QThread {
Q_OBJECT
class Impl;
using PImpl = std::unique_ptr<Impl>;
public:
SnakeThread(QObject *parent = nullptr);
~SnakeThread() override;
void setMeasurementArea(const QList<QGeoCoordinate> &measurementArea);
void setServiceArea(const QList<QGeoCoordinate> &serviceArea);
void setCorridor(const QList<QGeoCoordinate> &corridor);
void setProgress(const QVector<int> &progress);
SnakeTiles tiles() const;
SnakeTilesLocal tilesENU() const;
QGeoCoordinate ENUOrigin() const;
QVariantList tileCenterPoints() const;
QVector<int> progress() const;
bool calcInProgress() const;
QString errorMessage() const;
bool success() const;
bool tilesUpdated();
bool waypointsUpdated();
bool progressUpdated();
QVector<QGeoCoordinate> waypoints() const;
QVector<QGeoCoordinate> arrivalPath() const;
QVector<QGeoCoordinate> returnPath() const;
Length lineDistance() const;
void setLineDistance(Length lineDistance);
Length minTransectLength() const;
void setMinTransectLength(Length minTransectLength);
Area minTileArea() const;
void setMinTileArea(Area minTileArea);
Length tileHeight() const;
void setTileHeight(Length tileHeight);
Length tileWidth() const;
void setTileWidth(Length tileWidth);
signals:
void calcInProgressChanged(bool inProgress);
protected:
void run() override;
private:
PImpl pImpl;
};
src/Wima/Snake/SnakeTile.cpp deleted 100644 → 0
View file @ 458ab210
#include "SnakeTile.h"
SnakeTile::SnakeTile(QObject *parent) : WimaAreaData(parent) {}
SnakeTile::SnakeTile(const SnakeTile &other, QObject *parent)
: WimaAreaData(parent) {
*this = other;
}
SnakeTile::~SnakeTile() {}
QString SnakeTile::mapVisualQML() const {
return QStringLiteral("WimaAreaNoVisual.qml");
}
QString SnakeTile::type() const { return "Tile"; }
SnakeTile *SnakeTile::Clone() const { return new SnakeTile(*this); }
SnakeTile &SnakeTile::operator=(const SnakeTile &other) {
this->WimaAreaData::operator=(other);
return *this;
}
src/Wima/Snake/SnakeTile.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "Wima/Geometry/WimaAreaData.h"
class SnakeTile : public WimaAreaData {
Q_OBJECT
public:
SnakeTile(QObject *parent = nullptr);
SnakeTile(const SnakeTile &other, QObject *parent = nullptr);
~SnakeTile();
virtual QString mapVisualQML() const override;
QString type() const override;
SnakeTile *Clone() const;
SnakeTile &operator=(const SnakeTile &other);
protected:
void assign(const SnakeTile &other);
};
src/Wima/Snake/SnakeTileLocal.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "Wima/Geometry/GenericPolygon.h"
using SnakeTileLocal = GenericPolygon<QPointF, std::vector>;
src/Wima/Snake/SnakeTiles.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "SnakeTile.h"
#include "Wima/Geometry/GenericPolygonArray.h"
using SnakeTiles = GenericPolygonArray<SnakeTile, QVector>;
src/Wima/Snake/SnakeTilesLocal.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "Wima/Geometry/GenericPolygonArray.h"
#include "Wima/Snake/SnakeTileLocal.h"
#include <vector>
typedef GenericPolygonArray<SnakeTileLocal, std::vector> SnakeTilesLocal;
src/Wima/Snake/WimaAreaNoVisual.qml deleted 100644 → 0
View file @ 458ab210
import QtQuick 2.3
Item {
id: _root
property var map ///< Map control to place item in
property var qgcView ///< QGCView to use for popping dialogs
Component.onCompleted: {
console.log("WimaAreaNoVisual.qml is a place holder and not meant to be instanciated.")
}
}
src/Wima/Snake/clipper/clipper.cpp deleted 100644 → 0
View file @ 458ab210
This source diff could not be displayed because it is too large. You can view the blob instead.
src/Wima/Snake/clipper/clipper.hpp deleted 100644 → 0
View file @ 458ab210
/*******************************************************************************
* *
* Author : Angus Johnson *
* Version : 6.4.2 *
* Date : 27 February 2017 *
* Website : http://www.angusj.com *
* Copyright : Angus Johnson 2010-2017 *
* *
* License: *
* Use, modification & distribution is subject to Boost Software License Ver 1. *
* http://www.boost.org/LICENSE_1_0.txt *
* *
* Attributions: *
* The code in this library is an extension of Bala Vatti's clipping algorithm: *
* "A generic solution to polygon clipping" *
* Communications of the ACM, Vol 35, Issue 7 (July 1992) pp 56-63. *
* http://portal.acm.org/citation.cfm?id=129906 *
* *
* Computer graphics and geometric modeling: implementation and algorithms *
* By Max K. Agoston *
* Springer; 1 edition (January 4, 2005) *
* http://books.google.com/books?q=vatti+clipping+agoston *
* *
* See also: *
* "Polygon Offsetting by Computing Winding Numbers" *
* Paper no. DETC2005-85513 pp. 565-575 *
* ASME 2005 International Design Engineering Technical Conferences *
* and Computers and Information in Engineering Conference (IDETC/CIE2005) *
* September 24-28, 2005 , Long Beach, California, USA *
* http://www.me.berkeley.edu/~mcmains/pubs/DAC05OffsetPolygon.pdf *
* *
*******************************************************************************/
#ifndef clipper_hpp
#define clipper_hpp
#define CLIPPER_VERSION "6.4.2"
//use_int32: When enabled 32bit ints are used instead of 64bit ints. This
//improve performance but coordinate values are limited to the range +/- 46340
//#define use_int32
//use_xyz: adds a Z member to IntPoint. Adds a minor cost to perfomance.
//#define use_xyz
//use_lines: Enables line clipping. Adds a very minor cost to performance.
#define use_lines
//use_deprecated: Enables temporary support for the obsolete functions
//#define use_deprecated
#include <vector>
#include <list>
#include <set>
#include <stdexcept>
#include <cstring>
#include <cstdlib>
#include <ostream>
#include <functional>
#include <queue>
namespace ClipperLib {
enum ClipType { ctIntersection, ctUnion, ctDifference, ctXor };
enum PolyType { ptSubject, ptClip };
//By far the most widely used winding rules for polygon filling are
//EvenOdd & NonZero (GDI, GDI+, XLib, OpenGL, Cairo, AGG, Quartz, SVG, Gr32)
//Others rules include Positive, Negative and ABS_GTR_EQ_TWO (only in OpenGL)
//see http://glprogramming.com/red/chapter11.html
enum PolyFillType { pftEvenOdd, pftNonZero, pftPositive, pftNegative };
#ifdef use_int32
typedef int cInt;
static cInt const loRange = 0x7FFF;
static cInt const hiRange = 0x7FFF;
#else
typedef signed long long cInt;
static cInt const loRange = 0x3FFFFFFF;
static cInt const hiRange = 0x3FFFFFFFFFFFFFFFLL;
typedef signed long long long64; //used by Int128 class
typedef unsigned long long ulong64;
#endif
struct IntPoint {
cInt X;
cInt Y;
#ifdef use_xyz
cInt Z;
IntPoint(cInt x = 0, cInt y = 0, cInt z = 0): X(x), Y(y), Z(z) {};
#else
IntPoint(cInt x = 0, cInt y = 0): X(x), Y(y) {};
#endif
friend inline bool operator== (const IntPoint& a, const IntPoint& b)
{
return a.X == b.X && a.Y == b.Y;
}
friend inline bool operator!= (const IntPoint& a, const IntPoint& b)
{
return a.X != b.X || a.Y != b.Y;
}
};
//------------------------------------------------------------------------------
typedef std::vector< IntPoint > Path;
typedef std::vector< Path > Paths;
inline Path& operator <<(Path& poly, const IntPoint& p) {poly.push_back(p); return poly;}
inline Paths& operator <<(Paths& polys, const Path& p) {polys.push_back(p); return polys;}
std::ostream& operator <<(std::ostream &s, const IntPoint &p);
std::ostream& operator <<(std::ostream &s, const Path &p);
std::ostream& operator <<(std::ostream &s, const Paths &p);
struct DoublePoint
{
double X;
double Y;
DoublePoint(double x = 0, double y = 0) : X(x), Y(y) {}
DoublePoint(IntPoint ip) : X((double)ip.X), Y((double)ip.Y) {}
};
//------------------------------------------------------------------------------
#ifdef use_xyz
typedef void (*ZFillCallback)(IntPoint& e1bot, IntPoint& e1top, IntPoint& e2bot, IntPoint& e2top, IntPoint& pt);
#endif
enum InitOptions {ioReverseSolution = 1, ioStrictlySimple = 2, ioPreserveCollinear = 4};
enum JoinType {jtSquare, jtRound, jtMiter};
enum EndType {etClosedPolygon, etClosedLine, etOpenButt, etOpenSquare, etOpenRound};
class PolyNode;
typedef std::vector< PolyNode* > PolyNodes;
class PolyNode
{
public:
PolyNode();
virtual ~PolyNode(){};
Path Contour;
PolyNodes Childs;
PolyNode* Parent;
PolyNode* GetNext() const;
bool IsHole() const;
bool IsOpen() const;
int ChildCount() const;
private:
//PolyNode& operator =(PolyNode& other);
unsigned Index; //node index in Parent.Childs
bool m_IsOpen;
JoinType m_jointype;
EndType m_endtype;
PolyNode* GetNextSiblingUp() const;
void AddChild(PolyNode& child);
friend class Clipper; //to access Index
friend class ClipperOffset;
};
class PolyTree: public PolyNode
{
public:
~PolyTree(){ Clear(); };
PolyNode* GetFirst() const;
void Clear();
int Total() const;
private:
//PolyTree& operator =(PolyTree& other);
PolyNodes AllNodes;
friend class Clipper; //to access AllNodes
};
bool Orientation(const Path &poly);
double Area(const Path &poly);
int PointInPolygon(const IntPoint &pt, const Path &path);
void SimplifyPolygon(const Path &in_poly, Paths &out_polys, PolyFillType fillType = pftEvenOdd);
void SimplifyPolygons(const Paths &in_polys, Paths &out_polys, PolyFillType fillType = pftEvenOdd);
void SimplifyPolygons(Paths &polys, PolyFillType fillType = pftEvenOdd);
void CleanPolygon(const Path& in_poly, Path& out_poly, double distance = 1.415);
void CleanPolygon(Path& poly, double distance = 1.415);
void CleanPolygons(const Paths& in_polys, Paths& out_polys, double distance = 1.415);
void CleanPolygons(Paths& polys, double distance = 1.415);
void MinkowskiSum(const Path& pattern, const Path& path, Paths& solution, bool pathIsClosed);
void MinkowskiSum(const Path& pattern, const Paths& paths, Paths& solution, bool pathIsClosed);
void MinkowskiDiff(const Path& poly1, const Path& poly2, Paths& solution);
void PolyTreeToPaths(const PolyTree& polytree, Paths& paths);
void ClosedPathsFromPolyTree(const PolyTree& polytree, Paths& paths);
void OpenPathsFromPolyTree(PolyTree& polytree, Paths& paths);
void ReversePath(Path& p);
void ReversePaths(Paths& p);
struct IntRect { cInt left; cInt top; cInt right; cInt bottom; };
//enums that are used internally ...
enum EdgeSide { esLeft = 1, esRight = 2};
//forward declarations (for stuff used internally) ...
struct TEdge;
struct IntersectNode;
struct LocalMinimum;
struct OutPt;
struct OutRec;
struct Join;
typedef std::vector < OutRec* > PolyOutList;
typedef std::vector < TEdge* > EdgeList;
typedef std::vector < Join* > JoinList;
typedef std::vector < IntersectNode* > IntersectList;
//------------------------------------------------------------------------------
//ClipperBase is the ancestor to the Clipper class. It should not be
//instantiated directly. This class simply abstracts the conversion of sets of
//polygon coordinates into edge objects that are stored in a LocalMinima list.
class ClipperBase
{
public:
ClipperBase();
virtual ~ClipperBase();
virtual bool AddPath(const Path &pg, PolyType PolyTyp, bool Closed);
bool AddPaths(const Paths &ppg, PolyType PolyTyp, bool Closed);
virtual void Clear();
IntRect GetBounds();
bool PreserveCollinear() {return m_PreserveCollinear;};
void PreserveCollinear(bool value) {m_PreserveCollinear = value;};
protected:
void DisposeLocalMinimaList();
TEdge* AddBoundsToLML(TEdge *e, bool IsClosed);
virtual void Reset();
TEdge* ProcessBound(TEdge* E, bool IsClockwise);
void InsertScanbeam(const cInt Y);
bool PopScanbeam(cInt &Y);
bool LocalMinimaPending();
bool PopLocalMinima(cInt Y, const LocalMinimum *&locMin);
OutRec* CreateOutRec();
void DisposeAllOutRecs();
void DisposeOutRec(PolyOutList::size_type index);
void SwapPositionsInAEL(TEdge *edge1, TEdge *edge2);
void DeleteFromAEL(TEdge *e);
void UpdateEdgeIntoAEL(TEdge *&e);
typedef std::vector<LocalMinimum> MinimaList;
MinimaList::iterator m_CurrentLM;
MinimaList m_MinimaList;
bool m_UseFullRange;
EdgeList m_edges;
bool m_PreserveCollinear;
bool m_HasOpenPaths;
PolyOutList m_PolyOuts;
TEdge *m_ActiveEdges;
typedef std::priority_queue<cInt> ScanbeamList;
ScanbeamList m_Scanbeam;
};
//------------------------------------------------------------------------------
class Clipper : public virtual ClipperBase
{
public:
Clipper(int initOptions = 0);
bool Execute(ClipType clipType,
Paths &solution,
PolyFillType fillType = pftEvenOdd);
bool Execute(ClipType clipType,
Paths &solution,
PolyFillType subjFillType,
PolyFillType clipFillType);
bool Execute(ClipType clipType,
PolyTree &polytree,
PolyFillType fillType = pftEvenOdd);
bool Execute(ClipType clipType,
PolyTree &polytree,
PolyFillType subjFillType,
PolyFillType clipFillType);
bool ReverseSolution() { return m_ReverseOutput; };
void ReverseSolution(bool value) {m_ReverseOutput = value;};
bool StrictlySimple() {return m_StrictSimple;};
void StrictlySimple(bool value) {m_StrictSimple = value;};
//set the callback function for z value filling on intersections (otherwise Z is 0)
#ifdef use_xyz
void ZFillFunction(ZFillCallback zFillFunc);
#endif
protected:
virtual bool ExecuteInternal();
private:
JoinList m_Joins;
JoinList m_GhostJoins;
IntersectList m_IntersectList;
ClipType m_ClipType;
typedef std::list<cInt> MaximaList;
MaximaList m_Maxima;
TEdge *m_SortedEdges;
bool m_ExecuteLocked;
PolyFillType m_ClipFillType;
PolyFillType m_SubjFillType;
bool m_ReverseOutput;
bool m_UsingPolyTree;
bool m_StrictSimple;
#ifdef use_xyz
ZFillCallback m_ZFill; //custom callback
#endif
void SetWindingCount(TEdge& edge);
bool IsEvenOddFillType(const TEdge& edge) const;
bool IsEvenOddAltFillType(const TEdge& edge) const;
void InsertLocalMinimaIntoAEL(const cInt botY);
void InsertEdgeIntoAEL(TEdge *edge, TEdge* startEdge);
void AddEdgeToSEL(TEdge *edge);
bool PopEdgeFromSEL(TEdge *&edge);
void CopyAELToSEL();
void DeleteFromSEL(TEdge *e);
void SwapPositionsInSEL(TEdge *edge1, TEdge *edge2);
bool IsContributing(const TEdge& edge) const;
bool IsTopHorz(const cInt XPos);
void DoMaxima(TEdge *e);
void ProcessHorizontals();
void ProcessHorizontal(TEdge *horzEdge);
void AddLocalMaxPoly(TEdge *e1, TEdge *e2, const IntPoint &pt);
OutPt* AddLocalMinPoly(TEdge *e1, TEdge *e2, const IntPoint &pt);
OutRec* GetOutRec(int idx);
void AppendPolygon(TEdge *e1, TEdge *e2);
void IntersectEdges(TEdge *e1, TEdge *e2, IntPoint &pt);
OutPt* AddOutPt(TEdge *e, const IntPoint &pt);
OutPt* GetLastOutPt(TEdge *e);
bool ProcessIntersections(const cInt topY);
void BuildIntersectList(const cInt topY);
void ProcessIntersectList();
void ProcessEdgesAtTopOfScanbeam(const cInt topY);
void BuildResult(Paths& polys);
void BuildResult2(PolyTree& polytree);
void SetHoleState(TEdge *e, OutRec *outrec);
void DisposeIntersectNodes();
bool FixupIntersectionOrder();
void FixupOutPolygon(OutRec &outrec);
void FixupOutPolyline(OutRec &outrec);
bool IsHole(TEdge *e);
bool FindOwnerFromSplitRecs(OutRec &outRec, OutRec *&currOrfl);
void FixHoleLinkage(OutRec &outrec);
void AddJoin(OutPt *op1, OutPt *op2, const IntPoint offPt);
void ClearJoins();
void ClearGhostJoins();
void AddGhostJoin(OutPt *op, const IntPoint offPt);
bool JoinPoints(Join *j, OutRec* outRec1, OutRec* outRec2);
void JoinCommonEdges();
void DoSimplePolygons();
void FixupFirstLefts1(OutRec* OldOutRec, OutRec* NewOutRec);
void FixupFirstLefts2(OutRec* InnerOutRec, OutRec* OuterOutRec);
void FixupFirstLefts3(OutRec* OldOutRec, OutRec* NewOutRec);
#ifdef use_xyz
void SetZ(IntPoint& pt, TEdge& e1, TEdge& e2);
#endif
};
//------------------------------------------------------------------------------
class ClipperOffset
{
public:
ClipperOffset(double miterLimit = 2.0, double roundPrecision = 0.25);
~ClipperOffset();
void AddPath(const Path& path, JoinType joinType, EndType endType);
void AddPaths(const Paths& paths, JoinType joinType, EndType endType);
void Execute(Paths& solution, double delta);
void Execute(PolyTree& solution, double delta);
void Clear();
double MiterLimit;
double ArcTolerance;
private:
Paths m_destPolys;
Path m_srcPoly;
Path m_destPoly;
std::vector<DoublePoint> m_normals;
double m_delta, m_sinA, m_sin, m_cos;
double m_miterLim, m_StepsPerRad;
IntPoint m_lowest;
PolyNode m_polyNodes;
void FixOrientations();
void DoOffset(double delta);
void OffsetPoint(int j, int& k, JoinType jointype);
void DoSquare(int j, int k);
void DoMiter(int j, int k, double r);
void DoRound(int j, int k);
};
//------------------------------------------------------------------------------
class clipperException : public std::exception
{
public:
clipperException(const char* description): m_descr(description) {}
virtual ~clipperException() throw() {}
virtual const char* what() const throw() {return m_descr.c_str();}
private:
std::string m_descr;
};
//------------------------------------------------------------------------------
} //ClipperLib namespace
#endif //clipper_hpp
src/Wima/Snake/json/CircularGenerator.SettingsGroup.json deleted 100644 → 0
View file @ 458ab210
{
"version": 1,
"fileType": "FactMetaData",
"QGC.MetaData.Facts":
[
{
"name": "TransectDistance",
"shortDescription": "The distance between transects.",
"type": "double",
"units": "m",
"min": 0.3,
"decimalPlaces": 1,
"defaultValue": 5.0
},
{
"name": "DeltaAlpha",
"shortDescription": "Angle discretisation.",
"type": "double",
"units": "Deg",
"min": 0.3,
"max": 90,
"decimalPlaces": 1,
"defaultValue": 5.0
},
{
"name": "MinLength",
"shortDescription": "The minimal transect length.",
"type": "double",
"units": "m",
"min": 0.3,
"decimalPlaces": 1,
"defaultValue": 5.0
}
]
}
src/Wima/Snake/json/LinearGenerator.SettingsGroup.json deleted 100644 → 0
View file @ 458ab210
{
"version": 1,
"fileType": "FactMetaData",
"QGC.MetaData.Facts":
[
{
"name": "TransectDistance",
"shortDescription": "The distance between transects.",
"type": "double",
"units": "m",
"min": 0.3,
"decimalPlaces": 1,
"defaultValue": 5.0
},
{
"name": "Alpha",
"shortDescription": "Transect angle.",
"type": "double",
"units": "Deg",
"min": 0,
"max": 180,
"decimalPlaces": 1,
"defaultValue": 0.0
},
{
"name": "MinLength",
"shortDescription": "The minimal transect length.",
"type": "double",
"units": "m",
"min": 0.3,
"decimalPlaces": 1,
"defaultValue": 5.0
}
]
}
src/Wima/Snake/mapbox/feature.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <mapbox/geometry.hpp>
#include <mapbox/variant.hpp>
#include <cstdint>
#include <string>
#include <vector>
#include <unordered_map>
namespace mapbox {
namespace feature {
struct value;
struct null_value_t
{
};
constexpr bool operator==(const null_value_t&, const null_value_t&) { return true; }
constexpr bool operator!=(const null_value_t&, const null_value_t&) { return false; }
constexpr bool operator<(const null_value_t&, const null_value_t&) { return false; }
constexpr null_value_t null_value = null_value_t();
// Multiple numeric types (uint64_t, int64_t, double) are present in order to support
// the widest possible range of JSON numbers, which do not have a maximum range.
// Implementations that produce `value`s should use that order for type preference,
// using uint64_t for positive integers, int64_t for negative integers, and double
// for non-integers and integers outside the range of 64 bits.
using value_base = mapbox::util::variant<null_value_t, bool, uint64_t, int64_t, double, std::string,
mapbox::util::recursive_wrapper<std::vector<value>>,
mapbox::util::recursive_wrapper<std::unordered_map<std::string, value>>>;
struct value : value_base
{
using value_base::value_base;
};
using property_map = std::unordered_map<std::string, value>;
// The same considerations and requirement for numeric types apply as for `value_base`.
using identifier = mapbox::util::variant<null_value_t, uint64_t, int64_t, double, std::string>;
template <class T>
struct feature
{
using coordinate_type = T;
using geometry_type = mapbox::geometry::geometry<T>; // Fully qualified to avoid GCC -fpermissive error.
geometry_type geometry;
property_map properties;
identifier id;
feature()
: geometry(),
properties(),
id() {}
feature(geometry_type const& geom_)
: geometry(geom_),
properties(),
id() {}
feature(geometry_type&& geom_)
: geometry(std::move(geom_)),
properties(),
id() {}
feature(geometry_type const& geom_, property_map const& prop_)
: geometry(geom_), properties(prop_), id() {}
feature(geometry_type&& geom_, property_map&& prop_)
: geometry(std::move(geom_)),
properties(std::move(prop_)),
id() {}
feature(geometry_type const& geom_, property_map const& prop_, identifier const& id_)
: geometry(geom_),
properties(prop_),
id(id_) {}
feature(geometry_type&& geom_, property_map&& prop_, identifier&& id_)
: geometry(std::move(geom_)),
properties(std::move(prop_)),
id(std::move(id_)) {}
};
template <class T>
constexpr bool operator==(feature<T> const& lhs, feature<T> const& rhs)
{
return lhs.id == rhs.id && lhs.geometry == rhs.geometry && lhs.properties == rhs.properties;
}
template <class T>
constexpr bool operator!=(feature<T> const& lhs, feature<T> const& rhs)
{
return !(lhs == rhs);
}
template <class T, template <typename...> class Cont = std::vector>
struct feature_collection : Cont<feature<T>>
{
using coordinate_type = T;
using feature_type = feature<T>;
using container_type = Cont<feature_type>;
using size_type = typename container_type::size_type;
template <class... Args>
feature_collection(Args&&... args) : container_type(std::forward<Args>(args)...)
{
}
feature_collection(std::initializer_list<feature_type> args)
: container_type(std::move(args)) {}
};
} // namespace feature
} // namespace mapbox
src/Wima/Snake/mapbox/geometry.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <mapbox/geometry/point.hpp>
#include <mapbox/geometry/line_string.hpp>
#include <mapbox/geometry/polygon.hpp>
#include <mapbox/geometry/multi_point.hpp>
#include <mapbox/geometry/multi_line_string.hpp>
#include <mapbox/geometry/multi_polygon.hpp>
#include <mapbox/geometry/geometry.hpp>
#include <mapbox/geometry/point_arithmetic.hpp>
#include <mapbox/geometry/for_each_point.hpp>
#include <mapbox/geometry/envelope.hpp>
src/Wima/Snake/mapbox/geometry/box.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <mapbox/geometry/point.hpp>
namespace mapbox {
namespace geometry {
template <typename T>
struct box
{
using coordinate_type = T;
using point_type = point<coordinate_type>;
constexpr box(point_type const& min_, point_type const& max_)
: min(min_), max(max_)
{
}
point_type min;
point_type max;
};
template <typename T>
constexpr bool operator==(box<T> const& lhs, box<T> const& rhs)
{
return lhs.min == rhs.min && lhs.max == rhs.max;
}
template <typename T>
constexpr bool operator!=(box<T> const& lhs, box<T> const& rhs)
{
return lhs.min != rhs.min || lhs.max != rhs.max;
}
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/empty.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
namespace mapbox {
namespace geometry {
struct empty
{
}; // this Geometry type represents the empty point set, ∅, for the coordinate space (OGC Simple Features).
constexpr bool operator==(empty, empty) { return true; }
constexpr bool operator!=(empty, empty) { return false; }
constexpr bool operator<(empty, empty) { return false; }
constexpr bool operator>(empty, empty) { return false; }
constexpr bool operator<=(empty, empty) { return true; }
constexpr bool operator>=(empty, empty) { return true; }
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/envelope.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <mapbox/geometry/box.hpp>
#include <mapbox/geometry/for_each_point.hpp>
#include <limits>
namespace mapbox {
namespace geometry {
template <typename G, typename T = typename G::coordinate_type>
box<T> envelope(G const& geometry)
{
using limits = std::numeric_limits<T>;
T min_t = limits::has_infinity ? -limits::infinity() : limits::min();
T max_t = limits::has_infinity ? limits::infinity() : limits::max();
point<T> min(max_t, max_t);
point<T> max(min_t, min_t);
for_each_point(geometry, [&](point<T> const& point) {
if (min.x > point.x) min.x = point.x;
if (min.y > point.y) min.y = point.y;
if (max.x < point.x) max.x = point.x;
if (max.y < point.y) max.y = point.y;
});
return box<T>(min, max);
}
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/for_each_point.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <mapbox/geometry/geometry.hpp>
namespace mapbox {
namespace geometry {
template <typename F>
void for_each_point(mapbox::geometry::empty const&, F&&)
{
}
template <typename Point, typename F>
auto for_each_point(Point&& point, F&& f)
-> decltype(point.x, point.y, void())
{
f(std::forward<Point>(point));
}
template <typename Container, typename F>
auto for_each_point(Container&& container, F&& f)
-> decltype(container.begin(), container.end(), void());
template <typename... Types, typename F>
void for_each_point(mapbox::util::variant<Types...> const& geom, F&& f)
{
mapbox::util::variant<Types...>::visit(geom, [&](auto const& g) {
for_each_point(g, f);
});
}
template <typename... Types, typename F>
void for_each_point(mapbox::util::variant<Types...>& geom, F&& f)
{
mapbox::util::variant<Types...>::visit(geom, [&](auto& g) {
for_each_point(g, f);
});
}
template <typename Container, typename F>
auto for_each_point(Container&& container, F&& f)
-> decltype(container.begin(), container.end(), void())
{
for (auto& e : container)
{
for_each_point(e, f);
}
}
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/geometry.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <mapbox/geometry/empty.hpp>
#include <mapbox/geometry/point.hpp>
#include <mapbox/geometry/line_string.hpp>
#include <mapbox/geometry/polygon.hpp>
#include <mapbox/geometry/multi_point.hpp>
#include <mapbox/geometry/multi_line_string.hpp>
#include <mapbox/geometry/multi_polygon.hpp>
#include <mapbox/variant.hpp>
// stl
#include <vector>
namespace mapbox {
namespace geometry {
template <typename T, template <typename...> class Cont = std::vector>
struct geometry_collection;
template <typename T, template <typename...> class Cont = std::vector>
using geometry_base = mapbox::util::variant<empty,
point<T>,
line_string<T, Cont>,
polygon<T, Cont>,
multi_point<T, Cont>,
multi_line_string<T, Cont>,
multi_polygon<T, Cont>,
geometry_collection<T, Cont>>;
template <typename T, template <typename...> class Cont = std::vector>
struct geometry : geometry_base<T, Cont>
{
using coordinate_type = T;
using geometry_base<T>::geometry_base;
};
template <typename T, template <typename...> class Cont>
struct geometry_collection : Cont<geometry<T>>
{
using coordinate_type = T;
using geometry_type = geometry<T>;
using container_type = Cont<geometry_type>;
using size_type = typename container_type::size_type;
template <class... Args>
geometry_collection(Args&&... args) : container_type(std::forward<Args>(args)...)
{
}
geometry_collection(std::initializer_list<geometry_type> args)
: container_type(std::move(args)) {}
};
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/line_string.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
// mapbox
#include <mapbox/geometry/point.hpp>
// stl
#include <vector>
namespace mapbox {
namespace geometry {
template <typename T, template <typename...> class Cont = std::vector>
struct line_string : Cont<point<T>>
{
using coordinate_type = T;
using point_type = point<T>;
using container_type = Cont<point_type>;
using size_type = typename container_type::size_type;
template <class... Args>
line_string(Args&&... args) : container_type(std::forward<Args>(args)...)
{
}
line_string(std::initializer_list<point_type> args)
: container_type(std::move(args)) {}
};
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/multi_line_string.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
// mapbox
#include <mapbox/geometry/line_string.hpp>
// stl
#include <vector>
namespace mapbox {
namespace geometry {
template <typename T, template <typename...> class Cont = std::vector>
struct multi_line_string : Cont<line_string<T>>
{
using coordinate_type = T;
using line_string_type = line_string<T>;
using container_type = Cont<line_string_type>;
using size_type = typename container_type::size_type;
template <class... Args>
multi_line_string(Args&&... args) : container_type(std::forward<Args>(args)...)
{
}
multi_line_string(std::initializer_list<line_string_type> args)
: container_type(std::move(args)) {}
};
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/multi_point.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
// mapbox
#include <mapbox/geometry/point.hpp>
// stl
#include <vector>
namespace mapbox {
namespace geometry {
template <typename T, template <typename...> class Cont = std::vector>
struct multi_point : Cont<point<T>>
{
using coordinate_type = T;
using point_type = point<T>;
using container_type = Cont<point_type>;
using size_type = typename container_type::size_type;
template <class... Args>
multi_point(Args&&... args) : container_type(std::forward<Args>(args)...)
{
}
multi_point(std::initializer_list<point_type> args)
: container_type(std::move(args)) {}
};
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/multi_polygon.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
// mapbox
#include <mapbox/geometry/polygon.hpp>
// stl
#include <vector>
namespace mapbox {
namespace geometry {
template <typename T, template <typename...> class Cont = std::vector>
struct multi_polygon : Cont<polygon<T>>
{
using coordinate_type = T;
using polygon_type = polygon<T>;
using container_type = Cont<polygon_type>;
using size_type = typename container_type::size_type;
template <class... Args>
multi_polygon(Args&&... args) : container_type(std::forward<Args>(args)...)
{
}
multi_polygon(std::initializer_list<polygon_type> args)
: container_type(std::move(args)) {}
};
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/point.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
namespace mapbox {
namespace geometry {
template <typename T>
struct point
{
using coordinate_type = T;
constexpr point()
: x(), y()
{
}
constexpr point(T x_, T y_)
: x(x_), y(y_)
{
}
T x;
T y;
};
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
template <typename T>
constexpr bool operator==(point<T> const& lhs, point<T> const& rhs)
{
return lhs.x == rhs.x && lhs.y == rhs.y;
}
#pragma GCC diagnostic pop
template <typename T>
constexpr bool operator!=(point<T> const& lhs, point<T> const& rhs)
{
return !(lhs == rhs);
}
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
namespace mapbox {
namespace geometry {
template <typename T>
point<T> operator+(point<T> const& lhs, point<T> const& rhs)
{
return point<T>(lhs.x + rhs.x, lhs.y + rhs.y);
}
template <typename T>
point<T> operator+(point<T> const& lhs, T const& rhs)
{
return point<T>(lhs.x + rhs, lhs.y + rhs);
}
template <typename T>
point<T> operator-(point<T> const& lhs, point<T> const& rhs)
{
return point<T>(lhs.x - rhs.x, lhs.y - rhs.y);
}
template <typename T>
point<T> operator-(point<T> const& lhs, T const& rhs)
{
return point<T>(lhs.x - rhs, lhs.y - rhs);
}
template <typename T>
point<T> operator*(point<T> const& lhs, point<T> const& rhs)
{
return point<T>(lhs.x * rhs.x, lhs.y * rhs.y);
}
template <typename T>
point<T> operator*(point<T> const& lhs, T const& rhs)
{
return point<T>(lhs.x * rhs, lhs.y * rhs);
}
template <typename T>
point<T> operator/(point<T> const& lhs, point<T> const& rhs)
{
return point<T>(lhs.x / rhs.x, lhs.y / rhs.y);
}
template <typename T>
point<T> operator/(point<T> const& lhs, T const& rhs)
{
return point<T>(lhs.x / rhs, lhs.y / rhs);
}
template <typename T>
point<T>& operator+=(point<T>& lhs, point<T> const& rhs)
{
lhs.x += rhs.x;
lhs.y += rhs.y;
return lhs;
}
template <typename T>
point<T>& operator+=(point<T>& lhs, T const& rhs)
{
lhs.x += rhs;
lhs.y += rhs;
return lhs;
}
template <typename T>
point<T>& operator-=(point<T>& lhs, point<T> const& rhs)
{
lhs.x -= rhs.x;
lhs.y -= rhs.y;
return lhs;
}
template <typename T>
point<T>& operator-=(point<T>& lhs, T const& rhs)
{
lhs.x -= rhs;
lhs.y -= rhs;
return lhs;
}
template <typename T>
point<T>& operator*=(point<T>& lhs, point<T> const& rhs)
{
lhs.x *= rhs.x;
lhs.y *= rhs.y;
return lhs;
}
template <typename T>
point<T>& operator*=(point<T>& lhs, T const& rhs)
{
lhs.x *= rhs;
lhs.y *= rhs;
return lhs;
}
template <typename T>
point<T>& operator/=(point<T>& lhs, point<T> const& rhs)
{
lhs.x /= rhs.x;
lhs.y /= rhs.y;
return lhs;
}
template <typename T>
point<T>& operator/=(point<T>& lhs, T const& rhs)
{
lhs.x /= rhs;
lhs.y /= rhs;
return lhs;
}
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry/polygon.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
// mapbox
#include <mapbox/geometry/point.hpp>
// stl
#include <vector>
namespace mapbox {
namespace geometry {
template <typename T, template <typename...> class Cont = std::vector>
struct linear_ring : Cont<point<T>>
{
using coordinate_type = T;
using point_type = point<T>;
using container_type = Cont<point_type>;
using size_type = typename container_type::size_type;
template <class... Args>
linear_ring(Args&&... args) : container_type(std::forward<Args>(args)...)
{
}
linear_ring(std::initializer_list<point_type> args)
: container_type(std::move(args)) {}
};
template <typename T, template <typename...> class Cont = std::vector>
struct polygon : Cont<linear_ring<T>>
{
using coordinate_type = T;
using linear_ring_type = linear_ring<T>;
using container_type = Cont<linear_ring_type>;
using size_type = typename container_type::size_type;
template <class... Args>
polygon(Args&&... args) : container_type(std::forward<Args>(args)...)
{
}
polygon(std::initializer_list<linear_ring_type> args)
: container_type(std::move(args)) {}
};
} // namespace geometry
} // namespace mapbox
src/Wima/Snake/mapbox/geometry_io.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <mapbox/geometry/empty.hpp>
#include <mapbox/feature.hpp>
#include <iostream>
#include <string>
namespace mapbox {
namespace geometry {
std::ostream& operator<<(std::ostream& os, const empty&)
{
return os << "[]";
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const point<T>& point)
{
return os << "[" << point.x << "," << point.y << "]";
}
template <typename T, template <class, class...> class C, class... Args>
std::ostream& operator<<(std::ostream& os, const C<T, Args...>& cont)
{
os << "[";
for (auto it = cont.cbegin();;)
{
os << *it;
if (++it == cont.cend())
{
break;
}
os << ",";
}
return os << "]";
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const line_string<T>& geom)
{
return os << static_cast<typename line_string<T>::container_type>(geom);
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const linear_ring<T>& geom)
{
return os << static_cast<typename linear_ring<T>::container_type>(geom);
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const polygon<T>& geom)
{
return os << static_cast<typename polygon<T>::container_type>(geom);
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const multi_point<T>& geom)
{
return os << static_cast<typename multi_point<T>::container_type>(geom);
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const multi_line_string<T>& geom)
{
return os << static_cast<typename multi_line_string<T>::container_type>(geom);
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const multi_polygon<T>& geom)
{
return os << static_cast<typename multi_polygon<T>::container_type>(geom);
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const geometry<T>& geom)
{
geometry<T>::visit(geom, [&](const auto& g) { os << g; });
return os;
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const geometry_collection<T>& geom)
{
return os << static_cast<typename geometry_collection<T>::container_type>(geom);
}
} // namespace geometry
namespace feature {
std::ostream& operator<<(std::ostream& os, const null_value_t&)
{
return os << "[]";
}
} // namespace feature
} // namespace mapbox
src/Wima/Snake/mapbox/optional.hpp deleted 100644 → 0
View file @ 458ab210
#ifndef MAPBOX_UTIL_OPTIONAL_HPP
#define MAPBOX_UTIL_OPTIONAL_HPP
#pragma message("This implementation of optional is deprecated. See https://github.com/mapbox/variant/issues/64.")
#include <type_traits>
#include <utility>
#include "variant.hpp"
namespace mapbox {
namespace util {
template <typename T>
class optional
{
static_assert(!std::is_reference<T>::value, "optional doesn't support references");
struct none_type
{
};
variant<none_type, T> variant_;
public:
optional() = default;
optional(optional const& rhs)
{
if (this != &rhs)
{ // protect against invalid self-assignment
variant_ = rhs.variant_;
}
}
optional(T const& v) { variant_ = v; }
explicit operator bool() const noexcept { return variant_.template is<T>(); }
T const& get() const { return variant_.template get<T>(); }
T& get() { return variant_.template get<T>(); }
T const& operator*() const { return this->get(); }
T operator*() { return this->get(); }
optional& operator=(T const& v)
{
variant_ = v;
return *this;
}
optional& operator=(optional const& rhs)
{
if (this != &rhs)
{
variant_ = rhs.variant_;
}
return *this;
}
template <typename... Args>
void emplace(Args&&... args)
{
variant_ = T{std::forward<Args>(args)...};
}
void reset() { variant_ = none_type{}; }
}; // class optional
} // namespace util
} // namespace mapbox
#endif // MAPBOX_UTIL_OPTIONAL_HPP
src/Wima/Snake/mapbox/polylabel.hpp deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <mapbox/geometry/polygon.hpp>
#include <mapbox/geometry/envelope.hpp>
#include <mapbox/geometry/point.hpp>
#include <mapbox/geometry/point_arithmetic.hpp>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <queue>
namespace mapbox {
namespace detail {
// get squared distance from a point to a segment
template <class T>
T getSegDistSq(const geometry::point<T>& p,
const geometry::point<T>& a,
const geometry::point<T>& b) {
auto x = a.x;
auto y = a.y;
auto dx = b.x - x;
auto dy = b.y - y;
if (dx != 0 || dy != 0) {
auto t = ((p.x - x) * dx + (p.y - y) * dy) / (dx * dx + dy * dy);
if (t > 1) {
x = b.x;
y = b.y;
} else if (t > 0) {
x += dx * t;
y += dy * t;
}
}
dx = p.x - x;
dy = p.y - y;
return dx * dx + dy * dy;
}
// signed distance from point to polygon outline (negative if point is outside)
template <class T>
auto pointToPolygonDist(const geometry::point<T>& point, const geometry::polygon<T>& polygon) {
bool inside = false;
auto minDistSq = std::numeric_limits<double>::infinity();
for (const auto& ring : polygon) {
for (std::size_t i = 0, len = ring.size(), j = len - 1; i < len; j = i++) {
const auto& a = ring[i];
const auto& b = ring[j];
if ((a.y > point.y) != (b.y > point.y) &&
(point.x < (b.x - a.x) * (point.y - a.y) / (b.y - a.y) + a.x)) inside = !inside;
minDistSq = std::min(minDistSq, getSegDistSq(point, a, b));
}
}
return (inside ? 1 : -1) * std::sqrt(minDistSq);
}
template <class T>
struct Cell {
Cell(const geometry::point<T>& c_, T h_, const geometry::polygon<T>& polygon)
: c(c_),
h(h_),
d(pointToPolygonDist(c, polygon)),
max(d + h * std::sqrt(2))
{}
geometry::point<T> c; // cell center
T h; // half the cell size
T d; // distance from cell center to polygon
T max; // max distance to polygon within a cell
};
// get polygon centroid
template <class T>
Cell<T> getCentroidCell(const geometry::polygon<T>& polygon) {
T area = 0;
geometry::point<T> c { 0, 0 };
const auto& ring = polygon.at(0);
for (std::size_t i = 0, len = ring.size(), j = len - 1; i < len; j = i++) {
const geometry::point<T>& a = ring[i];
const geometry::point<T>& b = ring[j];
auto f = a.x * b.y - b.x * a.y;
c.x += (a.x + b.x) * f;
c.y += (a.y + b.y) * f;
area += f * 3;
}
return Cell<T>(area == 0 ? ring.at(0) : c / area, 0, polygon);
}
} // namespace detail
template <class T>
geometry::point<T> polylabel(const geometry::polygon<T>& polygon, T precision = 1, bool debug = false) {
using namespace detail;
// find the bounding box of the outer ring
const geometry::box<T> envelope = geometry::envelope(polygon.at(0));
const geometry::point<T> size {
envelope.max.x - envelope.min.x,
envelope.max.y - envelope.min.y
};
const T cellSize = std::min(size.x, size.y);
T h = cellSize / 2;
// a priority queue of cells in order of their "potential" (max distance to polygon)
auto compareMax = [] (const Cell<T>& a, const Cell<T>& b) {
return a.max < b.max;
};
using Queue = std::priority_queue<Cell<T>, std::vector<Cell<T>>, decltype(compareMax)>;
Queue cellQueue(compareMax);
if (cellSize == 0) {
return envelope.min;
}
// cover polygon with initial cells
for (T x = envelope.min.x; x < envelope.max.x; x += cellSize) {
for (T y = envelope.min.y; y < envelope.max.y; y += cellSize) {
cellQueue.push(Cell<T>({x + h, y + h}, h, polygon));
}
}
// take centroid as the first best guess
auto bestCell = getCentroidCell(polygon);
// special case for rectangular polygons
Cell<T> bboxCell(envelope.min + size / 2.0, 0, polygon);
if (bboxCell.d > bestCell.d) {
bestCell = bboxCell;
}
auto numProbes = cellQueue.size();
while (!cellQueue.empty()) {
// pick the most promising cell from the queue
auto cell = cellQueue.top();
cellQueue.pop();
// update the best cell if we found a better one
if (cell.d > bestCell.d) {
bestCell = cell;
if (debug) std::cout << "found best " << ::round(1e4 * cell.d) / 1e4 << " after " << numProbes << " probes" << std::endl;
}
// do not drill down further if there's no chance of a better solution
if (cell.max - bestCell.d <= precision) continue;
// split the cell into four cells
h = cell.h / 2;
cellQueue.push(Cell<T>({cell.c.x - h, cell.c.y - h}, h, polygon));
cellQueue.push(Cell<T>({cell.c.x + h, cell.c.y - h}, h, polygon));
cellQueue.push(Cell<T>({cell.c.x - h, cell.c.y + h}, h, polygon));
cellQueue.push(Cell<T>({cell.c.x + h, cell.c.y + h}, h, polygon));
numProbes += 4;
}
if (debug) {
std::cout << "num probes: " << numProbes << std::endl;
std::cout << "best distance: " << bestCell.d << std::endl;
}
return bestCell.c;
}
} // namespace mapbox
src/Wima/Snake/mapbox/recursive_wrapper.hpp deleted 100644 → 0
View file @ 458ab210
#ifndef MAPBOX_UTIL_RECURSIVE_WRAPPER_HPP
#define MAPBOX_UTIL_RECURSIVE_WRAPPER_HPP
// Based on variant/recursive_wrapper.hpp from boost.
//
// Original license:
//
// Copyright (c) 2002-2003
// Eric Friedman, Itay Maman
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#include <cassert>
#include <utility>
namespace mapbox {
namespace util {
template <typename T>
class recursive_wrapper
{
T* p_;
void assign(T const& rhs)
{
this->get() = rhs;
}
public:
using type = T;
/**
* Default constructor default initializes the internally stored value.
* For POD types this means nothing is done and the storage is
* uninitialized.
*
* @throws std::bad_alloc if there is insufficient memory for an object
* of type T.
* @throws any exception thrown by the default constructur of T.
*/
recursive_wrapper()
: p_(new T){};
~recursive_wrapper() noexcept { delete p_; };
recursive_wrapper(recursive_wrapper const& operand)
: p_(new T(operand.get())) {}
recursive_wrapper(T const& operand)
: p_(new T(operand)) {}
recursive_wrapper(recursive_wrapper&& operand)
: p_(new T(std::move(operand.get()))) {}
recursive_wrapper(T&& operand)
: p_(new T(std::move(operand))) {}
inline recursive_wrapper& operator=(recursive_wrapper const& rhs)
{
assign(rhs.get());
return *this;
}
inline recursive_wrapper& operator=(T const& rhs)
{
assign(rhs);
return *this;
}
inline void swap(recursive_wrapper& operand) noexcept
{
T* temp = operand.p_;
operand.p_ = p_;
p_ = temp;
}
recursive_wrapper& operator=(recursive_wrapper&& rhs) noexcept
{
swap(rhs);
return *this;
}
recursive_wrapper& operator=(T&& rhs)
{
get() = std::move(rhs);
return *this;
}
T& get()
{
assert(p_);
return *get_pointer();
}
T const& get() const
{
assert(p_);
return *get_pointer();
}
T* get_pointer() { return p_; }
const T* get_pointer() const { return p_; }
operator T const&() const { return this->get(); }
operator T&() { return this->get(); }
}; // class recursive_wrapper
template <typename T>
inline void swap(recursive_wrapper<T>& lhs, recursive_wrapper<T>& rhs) noexcept
{
lhs.swap(rhs);
}
} // namespace util
} // namespace mapbox
#endif // MAPBOX_UTIL_RECURSIVE_WRAPPER_HPP
src/Wima/Snake/mapbox/variant.hpp deleted 100644 → 0
View file @ 458ab210
#ifndef MAPBOX_UTIL_VARIANT_HPP
#define MAPBOX_UTIL_VARIANT_HPP
#include <cassert>
#include <cstddef> // size_t
#include <new> // operator new
#include <stdexcept> // runtime_error
#include <string>
#include <tuple>
#include <type_traits>
#include <typeinfo>
#include <utility>
#include "recursive_wrapper.hpp"
// clang-format off
// [[deprecated]] is only available in C++14, use this for the time being
#if __cplusplus <= 201103L
# ifdef __GNUC__
# define MAPBOX_VARIANT_DEPRECATED __attribute__((deprecated))
# elif defined(_MSC_VER)
# define MAPBOX_VARIANT_DEPRECATED __declspec(deprecated)
# else
# define MAPBOX_VARIANT_DEPRECATED
# endif
#else
# define MAPBOX_VARIANT_DEPRECATED [[deprecated]]
#endif
#ifdef _MSC_VER
// https://msdn.microsoft.com/en-us/library/bw1hbe6y.aspx
#ifdef NDEBUG
#define VARIANT_INLINE __forceinline
#else
#define VARIANT_INLINE __declspec(noinline)
#endif
#else
#ifdef NDEBUG
#define VARIANT_INLINE inline __attribute__((always_inline))
#else
#define VARIANT_INLINE __attribute__((noinline))
#endif
#endif
// clang-format on
#define VARIANT_MAJOR_VERSION 1
#define VARIANT_MINOR_VERSION 1
#define VARIANT_PATCH_VERSION 0
#define VARIANT_VERSION (VARIANT_MAJOR_VERSION * 100000) + (VARIANT_MINOR_VERSION * 100) + (VARIANT_PATCH_VERSION)
namespace mapbox {
namespace util {
// XXX This should derive from std::logic_error instead of std::runtime_error.
// See https://github.com/mapbox/variant/issues/48 for details.
class bad_variant_access : public std::runtime_error
{
public:
explicit bad_variant_access(const std::string& what_arg)
: runtime_error(what_arg) {}
explicit bad_variant_access(const char* what_arg)
: runtime_error(what_arg) {}
}; // class bad_variant_access
template <typename R = void>
struct MAPBOX_VARIANT_DEPRECATED static_visitor
{
using result_type = R;
protected:
static_visitor() {}
~static_visitor() {}
};
namespace detail {
static constexpr std::size_t invalid_value = std::size_t(-1);
template <typename T, typename... Types>
struct direct_type;
template <typename T, typename First, typename... Types>
struct direct_type<T, First, Types...>
{
static constexpr std::size_t index = std::is_same<T, First>::value
? sizeof...(Types)
: direct_type<T, Types...>::index;
};
template <typename T>
struct direct_type<T>
{
static constexpr std::size_t index = invalid_value;
};
template <typename T, typename... Types>
struct convertible_type;
template <typename T, typename First, typename... Types>
struct convertible_type<T, First, Types...>
{
static constexpr std::size_t index = std::is_convertible<T, First>::value
? sizeof...(Types)
: convertible_type<T, Types...>::index;
};
template <typename T>
struct convertible_type<T>
{
static constexpr std::size_t index = invalid_value;
};
template <typename T, typename... Types>
struct value_traits
{
using value_type = typename std::remove_reference<T>::type;
static constexpr std::size_t direct_index = direct_type<value_type, Types...>::index;
static constexpr bool is_direct = direct_index != invalid_value;
static constexpr std::size_t index = is_direct ? direct_index : convertible_type<value_type, Types...>::index;
static constexpr bool is_valid = index != invalid_value;
static constexpr std::size_t tindex = is_valid ? sizeof...(Types)-index : 0;
using target_type = typename std::tuple_element<tindex, std::tuple<void, Types...>>::type;
};
// check if T is in Types...
template <typename T, typename... Types>
struct has_type;
template <typename T, typename First, typename... Types>
struct has_type<T, First, Types...>
{
static constexpr bool value = std::is_same<T, First>::value || has_type<T, Types...>::value;
};
template <typename T>
struct has_type<T> : std::false_type
{
};
template <typename T, typename... Types>
struct is_valid_type;
template <typename T, typename First, typename... Types>
struct is_valid_type<T, First, Types...>
{
static constexpr bool value = std::is_convertible<T, First>::value || is_valid_type<T, Types...>::value;
};
template <typename T>
struct is_valid_type<T> : std::false_type
{
};
template <typename T, typename R = void>
struct enable_if_type
{
using type = R;
};
template <typename F, typename V, typename Enable = void>
struct result_of_unary_visit
{
using type = typename std::result_of<F(V&)>::type;
};
template <typename F, typename V>
struct result_of_unary_visit<F, V, typename enable_if_type<typename F::result_type>::type>
{
using type = typename F::result_type;
};
template <typename F, typename V, typename Enable = void>
struct result_of_binary_visit
{
using type = typename std::result_of<F(V&, V&)>::type;
};
template <typename F, typename V>
struct result_of_binary_visit<F, V, typename enable_if_type<typename F::result_type>::type>
{
using type = typename F::result_type;
};
template <std::size_t arg1, std::size_t... others>
struct static_max;
template <std::size_t arg>
struct static_max<arg>
{
static const std::size_t value = arg;
};
template <std::size_t arg1, std::size_t arg2, std::size_t... others>
struct static_max<arg1, arg2, others...>
{
static const std::size_t value = arg1 >= arg2 ? static_max<arg1, others...>::value : static_max<arg2, others...>::value;
};
template <typename... Types>
struct variant_helper;
template <typename T, typename... Types>
struct variant_helper<T, Types...>
{
VARIANT_INLINE static void destroy(const std::size_t type_index, void* data)
{
if (type_index == sizeof...(Types))
{
reinterpret_cast<T*>(data)->~T();
}
else
{
variant_helper<Types...>::destroy(type_index, data);
}
}
VARIANT_INLINE static void move(const std::size_t old_type_index, void* old_value, void* new_value)
{
if (old_type_index == sizeof...(Types))
{
new (new_value) T(std::move(*reinterpret_cast<T*>(old_value)));
}
else
{
variant_helper<Types...>::move(old_type_index, old_value, new_value);
}
}
VARIANT_INLINE static void copy(const std::size_t old_type_index, const void* old_value, void* new_value)
{
if (old_type_index == sizeof...(Types))
{
new (new_value) T(*reinterpret_cast<const T*>(old_value));
}
else
{
variant_helper<Types...>::copy(old_type_index, old_value, new_value);
}
}
};
template <>
struct variant_helper<>
{
VARIANT_INLINE static void destroy(const std::size_t, void*) {}
VARIANT_INLINE static void move(const std::size_t, void*, void*) {}
VARIANT_INLINE static void copy(const std::size_t, const void*, void*) {}
};
template <typename T>
struct unwrapper
{
static T const& apply_const(T const& obj) { return obj; }
static T& apply(T& obj) { return obj; }
};
template <typename T>
struct unwrapper<recursive_wrapper<T>>
{
static auto apply_const(recursive_wrapper<T> const& obj)
-> typename recursive_wrapper<T>::type const&
{
return obj.get();
}
static auto apply(recursive_wrapper<T>& obj)
-> typename recursive_wrapper<T>::type&
{
return obj.get();
}
};
template <typename T>
struct unwrapper<std::reference_wrapper<T>>
{
static auto apply_const(std::reference_wrapper<T> const& obj)
-> typename std::reference_wrapper<T>::type const&
{
return obj.get();
}
static auto apply(std::reference_wrapper<T>& obj)
-> typename std::reference_wrapper<T>::type&
{
return obj.get();
}
};
template <typename F, typename V, typename R, typename... Types>
struct dispatcher;
template <typename F, typename V, typename R, typename T, typename... Types>
struct dispatcher<F, V, R, T, Types...>
{
VARIANT_INLINE static R apply_const(V const& v, F&& f)
{
if (v.template is<T>())
{
return f(unwrapper<T>::apply_const(v.template get<T>()));
}
else
{
return dispatcher<F, V, R, Types...>::apply_const(v, std::forward<F>(f));
}
}
VARIANT_INLINE static R apply(V& v, F&& f)
{
if (v.template is<T>())
{
return f(unwrapper<T>::apply(v.template get<T>()));
}
else
{
return dispatcher<F, V, R, Types...>::apply(v, std::forward<F>(f));
}
}
};
template <typename F, typename V, typename R, typename T>
struct dispatcher<F, V, R, T>
{
VARIANT_INLINE static R apply_const(V const& v, F&& f)
{
return f(unwrapper<T>::apply_const(v.template get<T>()));
}
VARIANT_INLINE static R apply(V& v, F&& f)
{
return f(unwrapper<T>::apply(v.template get<T>()));
}
};
template <typename F, typename V, typename R, typename T, typename... Types>
struct binary_dispatcher_rhs;
template <typename F, typename V, typename R, typename T0, typename T1, typename... Types>
struct binary_dispatcher_rhs<F, V, R, T0, T1, Types...>
{
VARIANT_INLINE static R apply_const(V const& lhs, V const& rhs, F&& f)
{
if (rhs.template is<T1>()) // call binary functor
{
return f(unwrapper<T0>::apply_const(lhs.template get<T0>()),
unwrapper<T1>::apply_const(rhs.template get<T1>()));
}
else
{
return binary_dispatcher_rhs<F, V, R, T0, Types...>::apply_const(lhs, rhs, std::forward<F>(f));
}
}
VARIANT_INLINE static R apply(V& lhs, V& rhs, F&& f)
{
if (rhs.template is<T1>()) // call binary functor
{
return f(unwrapper<T0>::apply(lhs.template get<T0>()),
unwrapper<T1>::apply(rhs.template get<T1>()));
}
else
{
return binary_dispatcher_rhs<F, V, R, T0, Types...>::apply(lhs, rhs, std::forward<F>(f));
}
}
};
template <typename F, typename V, typename R, typename T0, typename T1>
struct binary_dispatcher_rhs<F, V, R, T0, T1>
{
VARIANT_INLINE static R apply_const(V const& lhs, V const& rhs, F&& f)
{
return f(unwrapper<T0>::apply_const(lhs.template get<T0>()),
unwrapper<T1>::apply_const(rhs.template get<T1>()));
}
VARIANT_INLINE static R apply(V& lhs, V& rhs, F&& f)
{
return f(unwrapper<T0>::apply(lhs.template get<T0>()),
unwrapper<T1>::apply(rhs.template get<T1>()));
}
};
template <typename F, typename V, typename R, typename T, typename... Types>
struct binary_dispatcher_lhs;
template <typename F, typename V, typename R, typename T0, typename T1, typename... Types>
struct binary_dispatcher_lhs<F, V, R, T0, T1, Types...>
{
VARIANT_INLINE static R apply_const(V const& lhs, V const& rhs, F&& f)
{
if (lhs.template is<T1>()) // call binary functor
{
return f(unwrapper<T1>::apply_const(lhs.template get<T1>()),
unwrapper<T0>::apply_const(rhs.template get<T0>()));
}
else
{
return binary_dispatcher_lhs<F, V, R, T0, Types...>::apply_const(lhs, rhs, std::forward<F>(f));
}
}
VARIANT_INLINE static R apply(V& lhs, V& rhs, F&& f)
{
if (lhs.template is<T1>()) // call binary functor
{
return f(unwrapper<T1>::apply(lhs.template get<T1>()),
unwrapper<T0>::apply(rhs.template get<T0>()));
}
else
{
return binary_dispatcher_lhs<F, V, R, T0, Types...>::apply(lhs, rhs, std::forward<F>(f));
}
}
};
template <typename F, typename V, typename R, typename T0, typename T1>
struct binary_dispatcher_lhs<F, V, R, T0, T1>
{
VARIANT_INLINE static R apply_const(V const& lhs, V const& rhs, F&& f)
{
return f(unwrapper<T1>::apply_const(lhs.template get<T1>()),
unwrapper<T0>::apply_const(rhs.template get<T0>()));
}
VARIANT_INLINE static R apply(V& lhs, V& rhs, F&& f)
{
return f(unwrapper<T1>::apply(lhs.template get<T1>()),
unwrapper<T0>::apply(rhs.template get<T0>()));
}
};
template <typename F, typename V, typename R, typename... Types>
struct binary_dispatcher;
template <typename F, typename V, typename R, typename T, typename... Types>
struct binary_dispatcher<F, V, R, T, Types...>
{
VARIANT_INLINE static R apply_const(V const& v0, V const& v1, F&& f)
{
if (v0.template is<T>())
{
if (v1.template is<T>())
{
return f(unwrapper<T>::apply_const(v0.template get<T>()),
unwrapper<T>::apply_const(v1.template get<T>())); // call binary functor
}
else
{
return binary_dispatcher_rhs<F, V, R, T, Types...>::apply_const(v0, v1, std::forward<F>(f));
}
}
else if (v1.template is<T>())
{
return binary_dispatcher_lhs<F, V, R, T, Types...>::apply_const(v0, v1, std::forward<F>(f));
}
return binary_dispatcher<F, V, R, Types...>::apply_const(v0, v1, std::forward<F>(f));
}
VARIANT_INLINE static R apply(V& v0, V& v1, F&& f)
{
if (v0.template is<T>())
{
if (v1.template is<T>())
{
return f(unwrapper<T>::apply(v0.template get<T>()),
unwrapper<T>::apply(v1.template get<T>())); // call binary functor
}
else
{
return binary_dispatcher_rhs<F, V, R, T, Types...>::apply(v0, v1, std::forward<F>(f));
}
}
else if (v1.template is<T>())
{
return binary_dispatcher_lhs<F, V, R, T, Types...>::apply(v0, v1, std::forward<F>(f));
}
return binary_dispatcher<F, V, R, Types...>::apply(v0, v1, std::forward<F>(f));
}
};
template <typename F, typename V, typename R, typename T>
struct binary_dispatcher<F, V, R, T>
{
VARIANT_INLINE static R apply_const(V const& v0, V const& v1, F&& f)
{
return f(unwrapper<T>::apply_const(v0.template get<T>()),
unwrapper<T>::apply_const(v1.template get<T>())); // call binary functor
}
VARIANT_INLINE static R apply(V& v0, V& v1, F&& f)
{
return f(unwrapper<T>::apply(v0.template get<T>()),
unwrapper<T>::apply(v1.template get<T>())); // call binary functor
}
};
// comparator functors
struct equal_comp
{
template <typename T>
bool operator()(T const& lhs, T const& rhs) const
{
return lhs == rhs;
}
};
struct less_comp
{
template <typename T>
bool operator()(T const& lhs, T const& rhs) const
{
return lhs < rhs;
}
};
template <typename Variant, typename Comp>
class comparer
{
public:
explicit comparer(Variant const& lhs) noexcept
: lhs_(lhs) {}
comparer& operator=(comparer const&) = delete;
// visitor
template <typename T>
bool operator()(T const& rhs_content) const
{
T const& lhs_content = lhs_.template get<T>();
return Comp()(lhs_content, rhs_content);
}
private:
Variant const& lhs_;
};
// True if Predicate matches for all of the types Ts
template <template <typename> class Predicate, typename... Ts>
struct static_all_of : std::is_same<std::tuple<std::true_type, typename Predicate<Ts>::type...>,
std::tuple<typename Predicate<Ts>::type..., std::true_type>>
{
};
// True if Predicate matches for none of the types Ts
template <template <typename> class Predicate, typename... Ts>
struct static_none_of : std::is_same<std::tuple<std::false_type, typename Predicate<Ts>::type...>,
std::tuple<typename Predicate<Ts>::type..., std::false_type>>
{
};
} // namespace detail
struct no_init
{
};
template <typename... Types>
class variant
{
static_assert(sizeof...(Types) > 0, "Template parameter type list of variant can not be empty");
static_assert(detail::static_none_of<std::is_reference, Types...>::value, "Variant can not hold reference types. Maybe use std::reference?");
private:
static const std::size_t data_size = detail::static_max<sizeof(Types)...>::value;
static const std::size_t data_align = detail::static_max<alignof(Types)...>::value;
using first_type = typename std::tuple_element<0, std::tuple<Types...>>::type;
using data_type = typename std::aligned_storage<data_size, data_align>::type;
using helper_type = detail::variant_helper<Types...>;
std::size_t type_index;
data_type data;
public:
VARIANT_INLINE variant() noexcept(std::is_nothrow_default_constructible<first_type>::value)
: type_index(sizeof...(Types)-1)
{
static_assert(std::is_default_constructible<first_type>::value, "First type in variant must be default constructible to allow default construction of variant");
new (&data) first_type();
}
VARIANT_INLINE variant(no_init) noexcept
: type_index(detail::invalid_value) {}
// http://isocpp.org/blog/2012/11/universal-references-in-c11-scott-meyers
template <typename T, typename Traits = detail::value_traits<T, Types...>,
typename Enable = typename std::enable_if<Traits::is_valid>::type>
VARIANT_INLINE variant(T&& val) noexcept(std::is_nothrow_constructible<typename Traits::target_type, T&&>::value)
: type_index(Traits::index)
{
new (&data) typename Traits::target_type(std::forward<T>(val));
}
VARIANT_INLINE variant(variant<Types...> const& old)
: type_index(old.type_index)
{
helper_type::copy(old.type_index, &old.data, &data);
}
VARIANT_INLINE variant(variant<Types...>&& old) noexcept(std::is_nothrow_move_constructible<std::tuple<Types...>>::value)
: type_index(old.type_index)
{
helper_type::move(old.type_index, &old.data, &data);
}
private:
VARIANT_INLINE void copy_assign(variant<Types...> const& rhs)
{
helper_type::destroy(type_index, &data);
type_index = detail::invalid_value;
helper_type::copy(rhs.type_index, &rhs.data, &data);
type_index = rhs.type_index;
}
VARIANT_INLINE void move_assign(variant<Types...>&& rhs)
{
helper_type::destroy(type_index, &data);
type_index = detail::invalid_value;
helper_type::move(rhs.type_index, &rhs.data, &data);
type_index = rhs.type_index;
}
public:
VARIANT_INLINE variant<Types...>& operator=(variant<Types...>&& other)
{
move_assign(std::move(other));
return *this;
}
VARIANT_INLINE variant<Types...>& operator=(variant<Types...> const& other)
{
copy_assign(other);
return *this;
}
// conversions
// move-assign
template <typename T>
VARIANT_INLINE variant<Types...>& operator=(T&& rhs) noexcept
{
variant<Types...> temp(std::forward<T>(rhs));
move_assign(std::move(temp));
return *this;
}
// copy-assign
template <typename T>
VARIANT_INLINE variant<Types...>& operator=(T const& rhs)
{
variant<Types...> temp(rhs);
copy_assign(temp);
return *this;
}
template <typename T>
VARIANT_INLINE bool is() const
{
static_assert(detail::has_type<T, Types...>::value, "invalid type in T in `is<T>()` for this variant");
return type_index == detail::direct_type<T, Types...>::index;
}
VARIANT_INLINE bool valid() const
{
return type_index != detail::invalid_value;
}
template <typename T, typename... Args>
VARIANT_INLINE void set(Args&&... args)
{
helper_type::destroy(type_index, &data);
type_index = detail::invalid_value;
new (&data) T(std::forward<Args>(args)...);
type_index = detail::direct_type<T, Types...>::index;
}
// get<T>()
template <typename T, typename std::enable_if<
(detail::direct_type<T, Types...>::index != detail::invalid_value)>::type* = nullptr>
VARIANT_INLINE T& get()
{
if (type_index == detail::direct_type<T, Types...>::index)
{
return *reinterpret_cast<T*>(&data);
}
else
{
throw bad_variant_access("in get<T>()");
}
}
template <typename T, typename std::enable_if<
(detail::direct_type<T, Types...>::index != detail::invalid_value)>::type* = nullptr>
VARIANT_INLINE T const& get() const
{
if (type_index == detail::direct_type<T, Types...>::index)
{
return *reinterpret_cast<T const*>(&data);
}
else
{
throw bad_variant_access("in get<T>()");
}
}
// get<T>() - T stored as recursive_wrapper<T>
template <typename T, typename std::enable_if<
(detail::direct_type<recursive_wrapper<T>, Types...>::index != detail::invalid_value)>::type* = nullptr>
VARIANT_INLINE T& get()
{
if (type_index == detail::direct_type<recursive_wrapper<T>, Types...>::index)
{
return (*reinterpret_cast<recursive_wrapper<T>*>(&data)).get();
}
else
{
throw bad_variant_access("in get<T>()");
}
}
template <typename T, typename std::enable_if<
(detail::direct_type<recursive_wrapper<T>, Types...>::index != detail::invalid_value)>::type* = nullptr>
VARIANT_INLINE T const& get() const
{
if (type_index == detail::direct_type<recursive_wrapper<T>, Types...>::index)
{
return (*reinterpret_cast<recursive_wrapper<T> const*>(&data)).get();
}
else
{
throw bad_variant_access("in get<T>()");
}
}
// get<T>() - T stored as std::reference_wrapper<T>
template <typename T, typename std::enable_if<
(detail::direct_type<std::reference_wrapper<T>, Types...>::index != detail::invalid_value)>::type* = nullptr>
VARIANT_INLINE T& get()
{
if (type_index == detail::direct_type<std::reference_wrapper<T>, Types...>::index)
{
return (*reinterpret_cast<std::reference_wrapper<T>*>(&data)).get();
}
else
{
throw bad_variant_access("in get<T>()");
}
}
template <typename T, typename std::enable_if<
(detail::direct_type<std::reference_wrapper<T const>, Types...>::index != detail::invalid_value)>::type* = nullptr>
VARIANT_INLINE T const& get() const
{
if (type_index == detail::direct_type<std::reference_wrapper<T const>, Types...>::index)
{
return (*reinterpret_cast<std::reference_wrapper<T const> const*>(&data)).get();
}
else
{
throw bad_variant_access("in get<T>()");
}
}
// This function is deprecated because it returns an internal index field.
// Use which() instead.
MAPBOX_VARIANT_DEPRECATED VARIANT_INLINE std::size_t get_type_index() const
{
return type_index;
}
VARIANT_INLINE int which() const noexcept
{
return static_cast<int>(sizeof...(Types)-type_index - 1);
}
// visitor
// unary
template <typename F, typename V, typename R = typename detail::result_of_unary_visit<F, first_type>::type>
auto VARIANT_INLINE static visit(V const& v, F&& f)
-> decltype(detail::dispatcher<F, V, R, Types...>::apply_const(v, std::forward<F>(f)))
{
return detail::dispatcher<F, V, R, Types...>::apply_const(v, std::forward<F>(f));
}
// non-const
template <typename F, typename V, typename R = typename detail::result_of_unary_visit<F, first_type>::type>
auto VARIANT_INLINE static visit(V& v, F&& f)
-> decltype(detail::dispatcher<F, V, R, Types...>::apply(v, std::forward<F>(f)))
{
return detail::dispatcher<F, V, R, Types...>::apply(v, std::forward<F>(f));
}
// binary
// const
template <typename F, typename V, typename R = typename detail::result_of_binary_visit<F, first_type>::type>
auto VARIANT_INLINE static binary_visit(V const& v0, V const& v1, F&& f)
-> decltype(detail::binary_dispatcher<F, V, R, Types...>::apply_const(v0, v1, std::forward<F>(f)))
{
return detail::binary_dispatcher<F, V, R, Types...>::apply_const(v0, v1, std::forward<F>(f));
}
// non-const
template <typename F, typename V, typename R = typename detail::result_of_binary_visit<F, first_type>::type>
auto VARIANT_INLINE static binary_visit(V& v0, V& v1, F&& f)
-> decltype(detail::binary_dispatcher<F, V, R, Types...>::apply(v0, v1, std::forward<F>(f)))
{
return detail::binary_dispatcher<F, V, R, Types...>::apply(v0, v1, std::forward<F>(f));
}
~variant() noexcept // no-throw destructor
{
helper_type::destroy(type_index, &data);
}
// comparison operators
// equality
VARIANT_INLINE bool operator==(variant const& rhs) const
{
assert(valid() && rhs.valid());
if (this->which() != rhs.which())
{
return false;
}
detail::comparer<variant, detail::equal_comp> visitor(*this);
return visit(rhs, visitor);
}
VARIANT_INLINE bool operator!=(variant const& rhs) const
{
return !(*this == rhs);
}
// less than
VARIANT_INLINE bool operator<(variant const& rhs) const
{
assert(valid() && rhs.valid());
if (this->which() != rhs.which())
{
return this->which() < rhs.which();
}
detail::comparer<variant, detail::less_comp> visitor(*this);
return visit(rhs, visitor);
}
VARIANT_INLINE bool operator>(variant const& rhs) const
{
return rhs < *this;
}
VARIANT_INLINE bool operator<=(variant const& rhs) const
{
return !(*this > rhs);
}
VARIANT_INLINE bool operator>=(variant const& rhs) const
{
return !(*this < rhs);
}
};
// unary visitor interface
// const
template <typename F, typename V>
auto VARIANT_INLINE apply_visitor(F&& f, V const& v) -> decltype(V::visit(v, std::forward<F>(f)))
{
return V::visit(v, std::forward<F>(f));
}
// non-const
template <typename F, typename V>
auto VARIANT_INLINE apply_visitor(F&& f, V& v) -> decltype(V::visit(v, std::forward<F>(f)))
{
return V::visit(v, std::forward<F>(f));
}
// binary visitor interface
// const
template <typename F, typename V>
auto VARIANT_INLINE apply_visitor(F&& f, V const& v0, V const& v1) -> decltype(V::binary_visit(v0, v1, std::forward<F>(f)))
{
return V::binary_visit(v0, v1, std::forward<F>(f));
}
// non-const
template <typename F, typename V>
auto VARIANT_INLINE apply_visitor(F&& f, V& v0, V& v1) -> decltype(V::binary_visit(v0, v1, std::forward<F>(f)))
{
return V::binary_visit(v0, v1, std::forward<F>(f));
}
// getter interface
template <typename ResultType, typename T>
ResultType& get(T& var)
{
return var.template get<ResultType>();
}
template <typename ResultType, typename T>
ResultType const& get(T const& var)
{
return var.template get<ResultType>();
}
} // namespace util
} // namespace mapbox
#endif // MAPBOX_UTIL_VARIANT_HPP
src/Wima/Snake/mapbox/variant_io.hpp deleted 100644 → 0
View file @ 458ab210
#ifndef MAPBOX_UTIL_VARIANT_IO_HPP
#define MAPBOX_UTIL_VARIANT_IO_HPP
#include <iosfwd>
#include "variant.hpp"
namespace mapbox {
namespace util {
namespace detail {
// operator<< helper
template <typename Out>
class printer
{
public:
explicit printer(Out& out)
: out_(out) {}
printer& operator=(printer const&) = delete;
// visitor
template <typename T>
void operator()(T const& operand) const
{
out_ << operand;
}
private:
Out& out_;
};
}
// operator<<
template <typename CharT, typename Traits, typename... Types>
VARIANT_INLINE std::basic_ostream<CharT, Traits>&
operator<<(std::basic_ostream<CharT, Traits>& out, variant<Types...> const& rhs)
{
detail::printer<std::basic_ostream<CharT, Traits>> visitor(out);
apply_visitor(visitor, rhs);
return out;
}
} // namespace util
} // namespace mapbox
#endif // MAPBOX_UTIL_VARIANT_IO_HPP
src/Wima/Snake/snake.cpp deleted 100644 → 0
View file @ 458ab210
#include <algorithm>
#include <iostream>
#include "snake.h"
#include <mapbox/geometry.hpp>
#include <mapbox/polylabel.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 1000000
#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"
using namespace operations_research;
#ifndef NDEBUG
//#define SNAKE_SHOW_TIME
#endif
namespace bg = boost::geometry;
namespace trans = bg::strategy::transform;
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(bg::cs::cartesian)
namespace snake {
static const IntType stdScale = 1000000;
//=========================================================================
// Geometry stuff.
//=========================================================================
void polygonCenter(const FPolygon &polygon, FPoint &center) {
using namespace mapbox;
if (polygon.outer().empty())
return;
geometry::polygon<double> p;
geometry::linear_ring<double> lr1;
for (size_t i = 0; i < polygon.outer().size(); ++i) {
geometry::point<double> vertex(polygon.outer()[i].get<0>(),
polygon.outer()[i].get<1>());
lr1.push_back(vertex);
}
p.push_back(lr1);
geometry::point<double> c = polylabel(p);
center.set<0>(c.x);
center.set<1>(c.y);
}
bool minimalBoundingBox(const FPolygon &polygon, BoundingBox &minBBox) {
/*
Find the minimum-area bounding box of a set of 2D points
The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates.
The first and last points points must be the same, making a closed polygon.
This program finds the rotation angles of each edge of the convex polygon,
then tests the area of a bounding box aligned with the unique angles in
90 degrees of the 1st Quadrant.
Returns the
Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version)
Results verified using Matlab
Copyright (c) 2013, David Butterworth, University of Queensland
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Willow Garage, Inc. nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
if (polygon.outer().empty() || polygon.outer().size() < 3)
return false;
FPolygon convex_hull;
bg::convex_hull(polygon, convex_hull);
// cout << "Convex hull: " << bg::wkt<BoostPolygon2D>(convex_hull) << endl;
//# Compute edges (x2-x1,y2-y1)
std::vector<FPoint> edges;
const auto &convex_hull_outer = convex_hull.outer();
for (long i = 0; i < long(convex_hull_outer.size()) - 1; ++i) {
FPoint p1 = convex_hull_outer.at(i);
FPoint p2 = convex_hull_outer.at(i + 1);
double edge_x = p2.get<0>() - p1.get<0>();
double edge_y = p2.get<1>() - p1.get<1>();
edges.push_back(FPoint{edge_x, edge_y});
}
// cout << "Edges: ";
// for (auto e : edges)
// cout << e.get<0>() << " " << e.get<1>() << ",";
// cout << endl;
// Calculate unique edge angles atan2(y/x)
double angle_scale = 1e3;
std::set<long> angles_long;
for (auto vertex : edges) {
double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2);
angle =
angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers
angles_long.insert(long(round(angle * angle_scale)));
}
std::vector<double> edge_angles;
for (auto a : angles_long)
edge_angles.push_back(double(a) / angle_scale);
// cout << "Unique angles: ";
// for (auto e : edge_angles)
// cout << e*180/M_PI << ",";
// cout << endl;
double min_area = std::numeric_limits<double>::infinity();
// Test each angle to find bounding box with smallest area
// print "Testing", len(edge_angles), "possible rotations for bounding box...
// \n"
for (double angle : edge_angles) {
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle * 180 /
M_PI);
FPolygon hull_rotated;
bg::transform(convex_hull, hull_rotated, rotate);
// cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated)
// << endl;
bg::model::box<FPoint> box;
bg::envelope(hull_rotated, box);
// cout << "Bounding box: " <<
// bg::wkt<bg::model::box<BoostPoint2D>>(box) << endl;
//# print "Rotated hull points are \n", rot_points
FPoint min_corner = box.min_corner();
FPoint max_corner = box.max_corner();
double min_x = min_corner.get<0>();
double max_x = max_corner.get<0>();
double min_y = min_corner.get<1>();
double max_y = max_corner.get<1>();
// cout << "min_x: " << min_x << endl;
// cout << "max_x: " << max_x << endl;
// cout << "min_y: " << min_y << endl;
// cout << "max_y: " << max_y << endl;
// Calculate height/width/area of this bounding rectangle
double width = max_x - min_x;
double height = max_y - min_y;
double area = width * height;
// cout << "Width: " << width << endl;
// cout << "Height: " << height << endl;
// cout << "area: " << area << endl;
// cout << "angle: " << angle*180/M_PI << endl;
// Store the smallest rect found first (a simple convex hull might have 2
// answers with same area)
if (area < min_area) {
min_area = area;
minBBox.angle = angle;
minBBox.width = width;
minBBox.height = height;
minBBox.corners.clear();
minBBox.corners.outer().push_back(FPoint{min_x, min_y});
minBBox.corners.outer().push_back(FPoint{min_x, max_y});
minBBox.corners.outer().push_back(FPoint{max_x, max_y});
minBBox.corners.outer().push_back(FPoint{max_x, min_y});
minBBox.corners.outer().push_back(FPoint{min_x, min_y});
}
// cout << endl << endl;
}
// Transform corners of minimal bounding box.
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-minBBox.angle *
180 / M_PI);
FPolygon rotated_polygon;
bg::transform(minBBox.corners, rotated_polygon, rotate);
minBBox.corners = rotated_polygon;
return true;
}
void offsetPolygon(const FPolygon &polygon, FPolygon &polygonOffset,
double offset) {
bg::strategy::buffer::distance_symmetric<double> distance_strategy(offset);
bg::strategy::buffer::join_miter join_strategy(3);
bg::strategy::buffer::end_flat end_strategy;
bg::strategy::buffer::point_square point_strategy;
bg::strategy::buffer::side_straight side_strategy;
bg::model::multi_polygon<FPolygon> result;
bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy,
end_strategy, point_strategy);
if (result.size() > 0)
polygonOffset = result[0];
}
void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices,
Matrix<double> &graph) {
size_t n = graph.n();
for (size_t i = 0; i < n; ++i) {
FPoint v1 = vertices[i];
for (size_t j = i + 1; j < n; ++j) {
FPoint v2 = vertices[j];
FLineString path{v1, v2};
double distance = 0;
if (!bg::within(path, polygon))
distance = std::numeric_limits<double>::infinity();
else
distance = bg::length(path);
graph(i, j) = distance;
graph(j, i) = distance;
}
}
}
bool toDistanceMatrix(Matrix<double> &graph) {
size_t n = graph.n();
auto distance = [&graph](size_t i, size_t j) -> double {
return graph(i, j);
};
for (size_t i = 0; i < n; ++i) {
for (size_t j = i + 1; j < n; ++j) {
double d = graph(i, j);
if (!std::isinf(d))
continue;
std::vector<size_t> path;
if (!dijkstraAlgorithm(n, i, j, path, d, distance)) {
return false;
}
// cout << "(" << i << "," << j << ") d: " << d << endl;
// cout << "Path size: " << path.size() << endl;
// for (auto idx : path)
// cout << idx << " ";
// cout << endl;
graph(i, j) = d;
graph(j, i) = d;
}
}
return true;
}
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles, BoundingBox &bbox,
string &errorString) {
if (area.outer().empty() || area.outer().size() < 4) {
errorString = "Area has to few vertices.";
return false;
}
if (tileWidth <= 0 * bu::si::meter || tileHeight <= 0 * bu::si::meter ||
minTileArea < 0 * bu::si::meter * bu::si::meter) {
std::stringstream ss;
ss << "Parameters tileWidth (" << tileWidth << "), tileHeight ("
<< tileHeight << "), minTileArea (" << minTileArea
<< ") must be positive.";
errorString = ss.str();
return false;
}
if (bbox.corners.outer().size() != 5) {
bbox.corners.clear();
minimalBoundingBox(area, bbox);
}
if (bbox.corners.outer().size() < 5)
return false;
double bboxWidth = bbox.width;
double bboxHeight = bbox.height;
FPoint origin = bbox.corners.outer()[0];
// cout << "Origin: " << origin[0] << " " << origin[1] << endl;
// Transform _mArea polygon to bounding box coordinate system.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(
bbox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(),
-origin.get<1>());
FPolygon translated_polygon;
FPolygon rotated_polygon;
boost::geometry::transform(area, translated_polygon, translate);
boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
bg::correct(rotated_polygon);
// cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t iMax = ceil(bboxWidth / tileWidth.value());
size_t jMax = ceil(bboxHeight / tileHeight.value());
if (iMax < 1 || jMax < 1) {
std::stringstream ss;
ss << "Tile width (" << tileWidth << ") or tile height (" << tileHeight
<< ") to large for measurement area.";
errorString = ss.str();
return false;
}
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(
-bbox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(),
origin.get<1>());
for (size_t i = 0; i < iMax; ++i) {
double x_min = tileWidth.value() * i;
double x_max = x_min + tileWidth.value();
for (size_t j = 0; j < jMax; ++j) {
double y_min = tileHeight.value() * j;
double y_max = y_min + tileHeight.value();
FPolygon tile_unclipped;
tile_unclipped.outer().push_back(FPoint{x_min, y_min});
tile_unclipped.outer().push_back(FPoint{x_min, y_max});
tile_unclipped.outer().push_back(FPoint{x_max, y_max});
tile_unclipped.outer().push_back(FPoint{x_max, y_min});
tile_unclipped.outer().push_back(FPoint{x_min, y_min});
std::deque<FPolygon> boost_tiles;
if (!boost::geometry::intersection(tile_unclipped, rotated_polygon,
boost_tiles))
continue;
for (FPolygon t : boost_tiles) {
if (bg::area(t) > minTileArea.value()) {
// Transform boost_tile to world coordinate system.
FPolygon rotated_tile;
FPolygon translated_tile;
boost::geometry::transform(t, rotated_tile, rotate_back);
boost::geometry::transform(rotated_tile, translated_tile,
translate_back);
// Store tile and calculate center point.
tiles.push_back(translated_tile);
}
}
}
}
if (tiles.size() < 1) {
std::stringstream ss;
ss << "No tiles calculated. Is the minTileArea (" << minTileArea
<< ") parameter large enough?";
errorString = ss.str();
return false;
}
return true;
}
bool joinedArea(const FPolygon &mArea, const FPolygon &sArea,
const FPolygon &corridor, FPolygon &jArea,
std::string &errorString) {
// Measurement area and service area overlapping?
bool overlapingSerMeas = bg::intersects(mArea, sArea) ? true : false;
bool corridorValid = corridor.outer().size() > 0 ? true : false;
// Check if corridor is connecting measurement area and service area.
bool corridor_is_connection = false;
if (corridorValid) {
// Corridor overlaping with measurement area?
if (bg::intersects(corridor, mArea)) {
// Corridor overlaping with service area?
if (bg::intersects(corridor, sArea)) {
corridor_is_connection = true;
}
}
}
// Are areas joinable?
std::deque<FPolygon> sol;
FPolygon partialArea = mArea;
if (overlapingSerMeas) {
if (corridor_is_connection) {
bg::union_(partialArea, corridor, sol);
}
} else if (corridor_is_connection) {
bg::union_(partialArea, corridor, sol);
} else {
std::stringstream ss;
auto printPoint = [&ss](const FPoint &p) {
ss << " (" << p.get<0>() << ", " << p.get<1>() << ")";
};
ss << "Areas are not overlapping." << std::endl;
ss << "Measurement area:";
bg::for_each_point(mArea, printPoint);
ss << std::endl;
ss << "Service area:";
bg::for_each_point(sArea, printPoint);
ss << std::endl;
ss << "Corridor:";
bg::for_each_point(corridor, printPoint);
ss << std::endl;
errorString = ss.str();
return false;
}
if (sol.size() > 0) {
partialArea = sol[0];
sol.clear();
}
// Join areas.
bg::union_(partialArea, sArea, sol);
if (sol.size() > 0) {
jArea = sol[0];
} else {
std::stringstream ss;
auto printPoint = [&ss](const FPoint &p) {
ss << " (" << p.get<0>() << ", " << p.get<1>() << ")";
};
ss << "Areas not joinable." << std::endl;
ss << "Measurement area:";
bg::for_each_point(mArea, printPoint);
ss << std::endl;
ss << "Service area:";
bg::for_each_point(sArea, printPoint);
ss << std::endl;
ss << "Corridor:";
bg::for_each_point(corridor, printPoint);
ss << std::endl;
errorString = ss.str();
return false;
}
return true;
}
bool joinedArea(const std::vector<FPolygon *> &areas, FPolygon &joinedArea) {
if (areas.size() < 1)
return false;
joinedArea = *areas[0];
std::deque<std::size_t> idxList;
for (size_t i = 1; i < areas.size(); ++i)
idxList.push_back(i);
std::deque<FPolygon> sol;
while (idxList.size() > 0) {
bool success = false;
for (auto it = idxList.begin(); it != idxList.end(); ++it) {
bg::union_(joinedArea, *areas[*it], sol);
if (sol.size() > 0) {
joinedArea = sol[0];
sol.clear();
idxList.erase(it);
success = true;
break;
}
}
if (!success)
return false;
}
return true;
}
BoundingBox::BoundingBox() : width(0), height(0), angle(0) {}
void BoundingBox::clear() {
width = 0;
height = 0;
angle = 0;
corners.clear();
}
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const FPolygon &mArea,
const std::vector<FPolygon> &tiles,
const Progress &p, Transects &t,
string &errorString) {
// Rotate measurement area by angle and calculate bounding box.
FPolygon mAreaRotated;
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
180 / M_PI);
bg::transform(mArea, mAreaRotated, rotate);
FBox box;
boost::geometry::envelope(mAreaRotated, box);
double x0 = box.min_corner().get<0>();
double y0 = box.min_corner().get<1>();
double x1 = box.max_corner().get<0>();
double y1 = box.max_corner().get<1>();
// Generate transects and convert them to clipper path.
size_t num_t = int(ceil((y1 - y0) / distance.value())); // number of transects
vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i = 0; i < num_t; ++i) {
// calculate transect
FPoint v1{x0, y0 + i * distance.value()};
FPoint v2{x1, y0 + i * distance.value()};
FLineString transect;
transect.push_back(v1);
transect.push_back(v2);
// transform back
FLineString temp_transect;
trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
-angle.value() * 180 / M_PI);
bg::transform(transect, temp_transect, rotate_back);
// to clipper
ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
temp_transect[0].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(
temp_transect[0].get<1>() * CLIPPER_SCALE)};
ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
temp_transect[1].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(
temp_transect[1].get<1>() * CLIPPER_SCALE)};
ClipperLib::Path path{c1, c2};
transectsClipper.push_back(path);
}
if (transectsClipper.size() == 0) {
std::stringstream ss;
ss << "Not able to generate transects. Parameter: distance = " << distance
<< std::endl;
errorString = ss.str();
return false;
}
// Convert measurement area to clipper path.
ClipperLib::Path mAreaClipper;
for (auto vertex : mArea.outer()) {
mAreaClipper.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib::Clipper clipper;
clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
ClipperLib::PolyTree clippedTransecs;
clipper.Execute(ClipperLib::ctIntersection, clippedTransecs,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
const auto *transects = &clippedTransecs;
bool ignoreProgress = p.size() != tiles.size();
ClipperLib::PolyTree clippedTransecs2;
if (!ignoreProgress) {
// Calculate processed tiles (_progress[i] == 100) and subtract them from
// measurement area.
size_t numTiles = p.size();
vector<FPolygon> processedTiles;
for (size_t i = 0; i < numTiles; ++i) {
if (p[i] == 100) {
processedTiles.push_back(tiles[i]);
}
}
if (processedTiles.size() != numTiles) {
vector<ClipperLib::Path> processedTilesClipper;
for (const auto &t : processedTiles) {
ClipperLib::Path path;
for (const auto &vertex : t.outer()) {
path.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
}
processedTilesClipper.push_back(path);
}
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper.Clear();
for (const auto &child : clippedTransecs.Childs) {
clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
}
clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
clipper.Execute(ClipperLib::ctDifference, clippedTransecs2,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
transects = &clippedTransecs2;
} else {
// All tiles processed (t.size() not changed).
return true;
}
}
// Extract transects from PolyTree and convert them to BoostLineString
for (const auto &child : transects->Childs) {
const auto &clipperTransect = child->Contour;
FPoint v1{static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
FPoint v2{static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};
FLineString transect{v1, v2};
if (bg::length(transect) >= minLength.value()) {
t.push_back(transect);
}
}
if (t.size() == 0) {
std::stringstream ss;
ss << "Not able to generate transects. Parameter: minLength = " << minLength
<< std::endl;
errorString = ss.str();
return false;
}
return true;
}
bool route(const FPolygon &area, const Transects &transects,
std::vector<Solution> &solutionVector, const RouteParameter &par) {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
//================================================================
// Create routing model.
//================================================================
// Use integer polygons to increase numerical robustness.
// Convert area;
IPolygon intArea;
for (const auto &v : area.outer()) {
auto p = float2Int(v);
intArea.outer().push_back(p);
}
for (const auto &ring : area.inners()) {
IRing intRing;
for (const auto &v : ring) {
auto p = float2Int(v);
intRing.push_back(p);
}
intArea.inners().push_back(std::move(intRing));
}
// Helper classes.
struct VirtualNode {
VirtualNode(std::size_t f, std::size_t t) : fromIndex(f), toIndex(t) {}
std::size_t fromIndex; // index for leaving node
std::size_t toIndex; // index for entering node
};
struct NodeToTransect {
NodeToTransect(std::size_t i, bool r) : transectsIndex(i), reversed(r) {}
std::size_t transectsIndex; // transects index
bool reversed; // transect reversed?
};
// Create vertex and node list
std::vector<IPoint> vertices;
std::vector<std::pair<std::size_t, std::size_t>> disjointNodes;
std::vector<VirtualNode> nodeList;
std::vector<NodeToTransect> nodeToTransectList;
for (std::size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
// Copy line edges only.
if (t.size() == 1 || i == 0) {
auto p = float2Int(t.back());
vertices.push_back(p);
nodeToTransectList.emplace_back(i, false);
auto idx = vertices.size() - 1;
nodeList.emplace_back(idx, idx);
} else if (t.size() > 1) {
auto p1 = float2Int(t.front());
auto p2 = float2Int(t.back());
vertices.push_back(p1);
vertices.push_back(p2);
nodeToTransectList.emplace_back(i, false);
nodeToTransectList.emplace_back(i, true);
auto fromIdx = vertices.size() - 1;
auto toIdx = fromIdx - 1;
nodeList.emplace_back(fromIdx, toIdx);
nodeList.emplace_back(toIdx, fromIdx);
disjointNodes.emplace_back(toIdx, fromIdx);
} else { // transect empty
std::cout << "ignoring empty transect with index " << i << std::endl;
}
}
#ifdef SNAKE_DEBUG
// Print.
std::cout << "nodeToTransectList:" << std::endl;
std::cout << "node:transectIndex:reversed" << std::endl;
std::size_t c = 0;
for (const auto &n2t : nodeToTransectList) {
std::cout << c++ << ":" << n2t.transectsIndex << ":" << n2t.reversed
<< std::endl;
}
std::cout << "nodeList:" << std::endl;
std::cout << "node:fromIndex:toIndex" << std::endl;
c = 0;
for (const auto &n : nodeList) {
std::cout << c++ << ":" << n.fromIndex << ":" << n.toIndex << std::endl;
}
std::cout << "disjoint nodes:" << std::endl;
std::cout << "number:nodes" << std::endl;
c = 0;
for (const auto &d : disjointNodes) {
std::cout << c++ << ":" << d.first << "," << d.second << std::endl;
}
#endif
// Add polygon vertices.
for (auto &v : intArea.outer()) {
vertices.push_back(v);
}
for (auto &ring : intArea.inners()) {
for (auto &v : ring) {
vertices.push_back(v);
}
}
// Create connection graph (inf == no connection between vertices).
// Note: graph is not symmetric.
auto n = vertices.size();
// Matrix must be double since integers don't have infinity and nan
Matrix<double> connectionGraph(n, n);
for (std::size_t i = 0; i < n; ++i) {
auto &fromVertex = vertices[i];
for (std::size_t j = 0; j < n; ++j) {
auto &toVertex = vertices[j];
ILineString line{fromVertex, toVertex};
if (bg::covered_by(line, intArea)) {
connectionGraph(i, j) = bg::length(line);
} else {
connectionGraph(i, j) = std::numeric_limits<double>::infinity();
}
}
}
#ifdef SNAKE_DEBUG
std::cout << "connection grah:" << std::endl;
std::cout << connectionGraph << std::endl;
#endif
// Create distance matrix.
auto distLambda = [&connectionGraph](std::size_t i, std::size_t j) -> double {
return connectionGraph(i, j);
};
auto nNodes = nodeList.size();
Matrix<IntType> distanceMatrix(nNodes, nNodes);
for (std::size_t i = 0; i < nNodes; ++i) {
distanceMatrix(i, i) = 0;
for (std::size_t j = i + 1; j < nNodes; ++j) {
auto dist = connectionGraph(i, j);
if (std::isinf(dist)) {
std::vector<std::size_t> route;
if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) {
std::stringstream ss;
ss << "Distance matrix calculation failed. connection graph: "
<< connectionGraph << std::endl;
ss << "area: " << bg::wkt(area) << std::endl;
ss << "transects:" << std::endl;
for (const auto &t : transects) {
ss << bg::wkt(t) << std::endl;
}
par.errorString = ss.str();
return false;
}
(void)route;
}
distanceMatrix(i, j) = dist;
distanceMatrix(j, i) = dist;
}
}
#ifdef SNAKE_DEBUG
std::cout << "distance matrix:" << std::endl;
std::cout << distanceMatrix << std::endl;
#endif
// Create (asymmetric) routing matrix.
Matrix<IntType> routingMatrix(nNodes, nNodes);
for (std::size_t i = 0; i < nNodes; ++i) {
auto fromNode = nodeList[i];
for (std::size_t j = 0; j < nNodes; ++j) {
auto toNode = nodeList[j];
routingMatrix(i, j) = distanceMatrix(fromNode.fromIndex, toNode.toIndex);
}
}
// Insert max for disjoint nodes.
for (const auto &d : disjointNodes) {
auto i = d.first;
auto j = d.second;
routingMatrix(i, j) = std::numeric_limits<IntType>::max();
routingMatrix(j, i) = std::numeric_limits<IntType>::max();
}
#ifdef SNAKE_DEBUG
std::cout << "routing matrix:" << std::endl;
std::cout << routingMatrix << std::endl;
#endif
// Create Routing Index Manager.
auto minNumTransectsPerRun =
std::max<std::size_t>(1, par.minNumTransectsPerRun);
auto maxRuns = std::max<std::size_t>(
1, std::floor(double(transects.size()) / minNumTransectsPerRun));
auto numRuns = std::max<std::size_t>(1, par.numRuns);
numRuns = std::min<std::size_t>(numRuns, maxRuns);
RoutingIndexManager::NodeIndex depot(0);
// std::vector<RoutingIndexManager::NodeIndex> depots(numRuns, depot);
// RoutingIndexManager manager(nNodes, numRuns, depots, depots);
RoutingIndexManager manager(nNodes, numRuns, depot);
// Create Routing Model.
RoutingModel routing(manager);
// Create and register a transit callback.
const int transitCallbackIndex = routing.RegisterTransitCallback(
[&routingMatrix, &manager](int64 from_index, int64 to_index) -> int64 {
// Convert from routing variable Index to distance matrix NodeIndex.
auto from_node = manager.IndexToNode(from_index).value();
auto to_node = manager.IndexToNode(to_index).value();
return routingMatrix(from_node, to_node);
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);
// Add distance dimension.
if (numRuns > 1) {
routing.AddDimension(transitCallbackIndex, 0, 300000000,
true, // start cumul to zero
"Distance");
routing.GetMutableDimension("Distance")
->SetGlobalSpanCostCoefficient(100000000);
}
// Define disjunctions.
#ifdef SNAKE_DEBUG
std::cout << "disjunctions:" << std::endl;
#endif
for (const auto &d : disjointNodes) {
auto i = d.first;
auto j = d.second;
#ifdef SNAKE_DEBUG
std::cout << i << "," << j << std::endl;
#endif
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(j));
std::vector<int64> disj{idx0, idx1};
routing.AddDisjunction(disj, -1 /*force cardinality*/, 1 /*cardinality*/);
}
// Set first solution heuristic.
auto searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
// Number of solutions.
auto numSolutionsPerRun = std::max<std::size_t>(1, par.numSolutionsPerRun);
searchParameters.set_number_of_solutions_to_collect(numSolutionsPerRun);
// Set costume limit.
auto *solver = routing.solver();
auto *limit = solver->MakeCustomLimit(par.stop);
routing.AddSearchMonitor(limit);
#ifdef SNAKE_SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "create routing model: " << delta.count() << " ms" << endl;
#endif
//================================================================
// Solve model.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
auto pSolutions = std::make_unique<std::vector<const Assignment *>>();
(void)routing.SolveWithParameters(searchParameters, pSolutions.get());
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "solve routing model: " << delta.count() << " ms" << endl;
#endif
if (par.stop()) {
par.errorString = "User terminated.";
return false;
}
if (pSolutions->size() == 0) {
std::stringstream ss;
ss << "No solution found." << std::endl;
par.errorString = ss.str();
return false;
}
//================================================================
// Construc route.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
long long counter = -1;
// Note: route number 0 corresponds to the best route which is the last entry
// of *pSolutions.
for (auto solution = pSolutions->end() - 1; solution >= pSolutions->begin();
--solution) {
++counter;
if (!*solution || (*solution)->Size() <= 1) {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << "invalid."
<< std::endl;
par.errorString = ss.str();
continue;
}
// Iterate over all routes.
Solution routeVector;
for (std::size_t vehicle = 0; vehicle < numRuns; ++vehicle) {
if (!routing.IsVehicleUsed(**solution, vehicle))
continue;
// Create index list.
auto index = routing.Start(vehicle);
std::vector<size_t> route_idx;
route_idx.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)) {
index = (*solution)->Value(routing.NextVar(index));
route_idx.push_back(manager.IndexToNode(index).value());
}
#ifdef SNAKE_DEBUG
// Print route.
std::cout << "route " << counter
<< " route_idx.size() = " << route_idx.size() << std::endl;
std::cout << "route: ";
for (const auto &idx : route_idx) {
std::cout << idx << ", ";
}
std::cout << std::endl;
#endif
if (route_idx.size() < 2) {
std::stringstream ss;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
// Assemble route.
Route r;
auto &path = r.path;
auto &info = r.info;
for (size_t i = 0; i < route_idx.size() - 1; ++i) {
size_t nodeIndex0 = route_idx[i];
size_t nodeIndex1 = route_idx[i + 1];
const auto &n2t0 = nodeToTransectList[nodeIndex0];
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
// Copy transect to route.
const auto &t = transects[n2t0.transectsIndex];
if (n2t0.reversed) { // transect reversal needed?
for (auto it = t.end() - 1; it > t.begin(); --it) {
path.push_back(*it);
}
} else {
for (auto it = t.begin(); it < t.end() - 1; ++it) {
path.push_back(*it);
}
}
// Connect transects.
std::vector<size_t> idxList;
if (!shortestPathFromGraph(connectionGraph,
nodeList[nodeIndex0].fromIndex,
nodeList[nodeIndex1].toIndex, idxList)) {
std::stringstream ss;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
if (i != route_idx.size() - 2) {
idxList.pop_back();
}
for (auto idx : idxList) {
auto p = int2Float(vertices[idx]);
path.push_back(p);
}
}
// Append last transect info.
const auto &n2t0 = nodeToTransectList.back();
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
if (path.size() < 2 || info.size() < 2) {
std::stringstream ss;
ss << par.errorString << "Route empty (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
routeVector.push_back(std::move(r));
}
if (routeVector.size() > 0) {
solutionVector.push_back(std::move(routeVector));
} else {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << " empty." << std::endl;
par.errorString = ss.str();
}
}
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "reconstruct route: " << delta.count() << " ms" << endl;
#endif
if (solutionVector.size() > 0) {
return true;
} else {
return false;
}
}
FPoint int2Float(const IPoint &ip) { return int2Float(ip, stdScale); }
FPoint int2Float(const IPoint &ip, IntType scale) {
return FPoint{FloatType(ip.get<0>()) / scale, FloatType(ip.get<1>()) / scale};
}
IPoint float2Int(const FPoint &ip) { return float2Int(ip, stdScale); }
IPoint float2Int(const FPoint &ip, IntType scale) {
return IPoint{IntType(std::llround(ip.get<0>() * scale)),
IntType(std::llround(ip.get<1>() * scale))};
}
bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex,
std::vector<size_t> &elementPath, double &length,
std::function<double(size_t, size_t)> distanceDij) {
if (startIndex >= numElements || endIndex >= numElements) {
length = std::numeric_limits<double>::infinity();
return false;
} else if (endIndex == startIndex) {
length = 0;
elementPath.push_back(startIndex);
return true;
}
// Node struct
// predecessorIndex is the index of the predecessor node
// (nodeList[predecessorIndex]) distance is the distance between the node and
// the start node node number is stored by the position in nodeList
struct Node {
std::size_t predecessorIndex = std::numeric_limits<std::size_t>::max();
double distance = std::numeric_limits<double>::infinity();
};
// The list with all Nodes (elements)
std::vector<Node> nodeList(numElements);
// This list will be initalized with indices referring to the elements of
// nodeList. Elements will be successively remove during the execution of the
// Dijkstra Algorithm.
std::vector<size_t> workingSet(numElements);
// append elements to node list
for (size_t i = 0; i < numElements; ++i)
workingSet[i] = i;
nodeList[startIndex].distance = 0;
// Dijkstra Algorithm
// https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
while (workingSet.size() > 0) {
// serach Node with minimal distance
auto minDist = std::numeric_limits<double>::infinity();
std::size_t minDistIndex_WS =
std::numeric_limits<std::size_t>::max(); // WS = workinSet
for (size_t i = 0; i < workingSet.size(); ++i) {
const auto nodeIndex = workingSet.at(i);
const auto dist = nodeList.at(nodeIndex).distance;
if (dist < minDist) {
minDist = dist;
minDistIndex_WS = i;
}
}
if (minDistIndex_WS == std::numeric_limits<std::size_t>::max())
return false;
size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList
workingSet.erase(workingSet.begin() + minDistIndex_WS);
if (indexU_NL == endIndex) // shortest path found
break;
const auto distanceU = nodeList.at(indexU_NL).distance;
// update distance
for (size_t i = 0; i < workingSet.size(); ++i) {
auto indexV_NL = workingSet[i]; // NL = nodeList
Node *v = &nodeList[indexV_NL];
auto dist = distanceDij(indexU_NL, indexV_NL);
// is ther an alternative path which is shorter?
auto alternative = distanceU + dist;
if (alternative < v->distance) {
v->distance = alternative;
v->predecessorIndex = indexU_NL;
}
}
}
// end Djikstra Algorithm
// reverse assemble path
auto e = endIndex;
length = nodeList[e].distance;
while (true) {
if (e == std::numeric_limits<std::size_t>::max()) {
if (elementPath.size() > 0 &&
elementPath[0] == startIndex) { // check if starting point was reached
break;
} else { // some error
length = std::numeric_limits<double>::infinity();
elementPath.clear();
return false;
}
} else {
elementPath.insert(elementPath.begin(), e);
// Update Node
e = nodeList[e].predecessorIndex;
}
}
return true;
}
bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
const size_t endIndex,
std::vector<size_t> &pathIdx) {
if (!std::isinf(graph(startIndex, endIndex))) {
pathIdx.push_back(startIndex);
pathIdx.push_back(endIndex);
} else {
auto distance = [&graph](size_t i, size_t j) -> double {
return graph(i, j);
};
double d = 0;
if (!dijkstraAlgorithm(graph.n(), startIndex, endIndex, pathIdx, d,
distance)) {
return false;
}
}
return true;
}
} // namespace snake
bool boost::geometry::model::operator==(snake::FPoint &p1, snake::FPoint &p2) {
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool boost::geometry::model::operator!=(snake::FPoint &p1, snake::FPoint &p2) {
return !(p1 == p2);
}
bool boost::geometry::model::operator==(snake::IPoint &p1, snake::IPoint &p2) {
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool boost::geometry::model::operator!=(snake::IPoint &p1, snake::IPoint &p2) {
return !(p1 == p2);
}
src/Wima/Snake/snake.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <array>
#include <atomic>
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include <boost/geometry.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
namespace bg = boost::geometry;
namespace bu = boost::units;
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
using namespace std;
namespace snake {
//=========================================================================
// Geometry stuff.
//=========================================================================
// Double geometry.
using FloatType = double;
typedef bg::model::point<double, 2, bg::cs::cartesian> FPoint;
typedef bg::model::polygon<FPoint> FPolygon;
typedef bg::model::linestring<FPoint> FLineString;
typedef bg::model::box<FPoint> FBox;
// Integer geometry.
using IntType = long long;
using IPoint = bg::model::point<IntType, 2, bg::cs::cartesian>;
using IPolygon = bg::model::polygon<IPoint>;
using IRing = bg::model::ring<IPoint>;
using ILineString = bg::model::linestring<IPoint>;
FPoint int2Float(const IPoint &ip);
FPoint int2Float(const IPoint &ip, IntType scale);
IPoint float2Int(const FPoint &ip);
IPoint float2Int(const FPoint &ip, IntType scale);
template <class T> class Matrix;
template <class DataType>
ostream &operator<<(ostream &os, const Matrix<DataType> &matrix) {
for (std::size_t i = 0; i < matrix.m(); ++i) {
for (std::size_t j = 0; j < matrix.n(); ++j) {
os << "(" << i << "," << j << "):" << matrix(i, j) << std::endl;
}
}
return os;
}
// Matrix
template <class DataType> class Matrix {
public:
using value_type = DataType;
Matrix(std::size_t m, std::size_t n) : _m(m), _n(n) { _matrix.resize(m * n); }
Matrix(std::size_t m, std::size_t n, DataType value) : _m(m), _n(n) {
_matrix.resize(m * n, value);
}
DataType &operator()(std::size_t i, std::size_t j) {
assert(i < _m);
assert(j < _n);
return _matrix[i * _m + j];
}
const DataType &operator()(std::size_t i, std::size_t j) const {
assert(i < _m);
assert(j < _n);
return _matrix[i * _m + j];
}
std::size_t m() const { return _n; }
std::size_t n() const { return _n; }
friend ostream &operator<<<>(ostream &os, const Matrix<DataType> &dt);
private:
std::vector<DataType> _matrix;
const std::size_t _m;
const std::size_t _n;
}; // Matrix
struct BoundingBox {
BoundingBox();
void clear();
double width;
double height;
double angle;
FPolygon corners;
};
template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth);
double x = 0, y = 0, z = 0;
auto alt = in.altitude();
alt = std::isnan(alt) ? 0 : alt;
proj.Forward(in.latitude(), in.longitude(), alt, x, y, z);
out.set<0>(x);
out.set<1>(y);
(void)z;
}
template <class GeoPoint1, class GeoPoint2, class Point>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth);
double x = 0, y = 0, z = 0;
auto alt = in.altitude();
alt = std::isnan(alt) ? 0 : alt;
proj.Forward(in.latitude(), in.longitude(), alt, x, y, z);
out.setX(x);
out.setY(y);
(void)z;
}
template <class GeoPoint>
void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth);
double lat = 0, lon = 0, alt = 0;
proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt);
out.setLatitude(lat);
out.setLongitude(lon);
out.setAltitude(alt);
}
template <class GeoPoint, class Container1, class Container2>
void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) {
for (auto &vertex : in) {
typename Container2::value_type p;
toENU(origin, vertex, p);
out.push_back(p);
}
}
template <class GeoPoint, class Container>
void areaToEnu(const GeoPoint &origin, const Container &in, FPolygon &out) {
for (auto &vertex : in) {
FPoint p;
toENU(origin, vertex, p);
out.outer().push_back(p);
}
bg::correct(out);
}
template <class GeoPoint, class Container1, class Container2>
void areaFromEnu(const GeoPoint &origin, Container1 &in,
const Container2 &out) {
for (auto &vertex : in) {
typename Container2::value_type p;
fromENU(origin, vertex, p);
out.push_back(p);
}
}
template <class GeoPoint, class Container>
void areaFromEnu(const GeoPoint &origin, FPolygon &in, const Container &out) {
for (auto &vertex : in.outer()) {
typename Container::value_type p;
fromENU(origin, vertex, p);
out.push_back(p);
}
}
void polygonCenter(const FPolygon &polygon, FPoint &center);
bool minimalBoundingBox(const FPolygon &polygon, BoundingBox &minBBox);
void offsetPolygon(const FPolygon &polygon, FPolygon &polygonOffset,
double offset);
void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices,
Matrix<double> &graph);
bool toDistanceMatrix(Matrix<double> &graph);
bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex,
std::vector<size_t> &elementPath, double &length,
std::function<double(size_t, size_t)> distanceDij);
bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
const size_t endIndex, std::vector<size_t> &pathIdx);
typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
typedef bu::quantity<bu::si::plane_angle> Angle;
typedef bu::quantity<bu::si::plane_angle> Radian;
typedef bu::quantity<bu::degree::plane_angle> Degree;
bool joinedArea(const std::vector<FPolygon *> &areas, FPolygon &jArea);
bool joinedArea(const FPolygon &mArea, const FPolygon &sArea,
const FPolygon &corridor, FPolygon &jArea,
std::string &errorString);
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles, BoundingBox &bbox,
std::string &errorString);
using Transects = vector<FLineString>;
using Progress = vector<int>;
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const FPolygon &mArea,
const std::vector<FPolygon> &tiles,
const Progress &p, Transects &t,
string &errorString);
struct TransectInfo {
TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
size_t index;
bool reversed;
};
struct Route {
FLineString path;
std::vector<TransectInfo> info;
};
using Solution =
std::vector<Route>; // Every route corresponds to one run/vehicle
struct RouteParameter {
RouteParameter()
: numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5),
stop([] { return false; }) {}
std::size_t numSolutionsPerRun;
std::size_t numRuns;
std::size_t minNumTransectsPerRun;
std::function<bool(void)> stop;
mutable std::string errorString;
};
bool route(const FPolygon &area, const Transects &transects,
std::vector<Solution> &solutionVector,
const RouteParameter &par = RouteParameter());
namespace detail {
const double offsetConstant =
0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
} // namespace snake
// operator== and operator!= for boost point
namespace boost {
namespace geometry {
namespace model {
bool operator==(snake::FPoint &p1, snake::FPoint &p2);
bool operator!=(snake::FPoint &p1, snake::FPoint &p2);
bool operator==(snake::IPoint &p1, snake::IPoint &p2);
bool operator!=(snake::IPoint &p1, snake::IPoint &p2);
} // namespace model
} // namespace geometry
} // namespace boost
src/Wima/Utils.cpp deleted 100644 → 0
View file @ 458ab210
#include "Utils.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include <iostream>
#include <QGeoCoordinate>
bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
long index,
QmlObjectListModel &list,
PlanMasterController* masterController,
bool flyView,
QObject *parent,
bool doUpdates)
{
using namespace WaypointManager::Utils;
if ( !coordinate.isValid() ){
return false;
} else {
if (parent == nullptr)
parent = &list;
auto *newItem = new SimpleMissionItem(masterController, flyView, MissionItem(), parent);
int sequenceNumber = detail::nextSequenceNumber(list);
newItem->setSequenceNumber(sequenceNumber);
newItem->setCoordinate(coordinate);
newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
// Set altitude and altitude mode from previous item.
if (newItem->specifiesAltitude()) {
double altitude = 10;
int altitudeMode = QGroundControlQmlGlobal::AltitudeModeRelative;
if (detail::previousAltitude(list, index-1, altitude, altitudeMode)) {
newItem->altitude()->setRawValue(altitude);
newItem->setAltitudeMode(
static_cast<QGroundControlQmlGlobal::AltitudeMode>(
altitudeMode) );
}
}
list.insert(index, newItem);
if ( doUpdates ) {
updateSequenceNumbers(list);
updateHirarchy(list);
updateHomePosition(list);
}
}
return true;
}
size_t WaypointManager::Utils::detail::nextSequenceNumber(QmlObjectListModel &list)
{
if (list.count() >= 1) {
VisualMissionItem* lastItem = list.value<VisualMissionItem*>(list.count() - 1);
return lastItem->lastSequenceNumber() + 1;
}
return 0;
}
bool WaypointManager::Utils::detail::previousAltitude(QmlObjectListModel &list,
size_t index,
double &altitude,
int &altitudeMode)
{
if (index >= size_t(list.count())) {
return false;
}
for (long i = index; i >= 0; --i) {
VisualMissionItem* visualItem = list.value<VisualMissionItem*>(i);
if ( visualItem->specifiesCoordinate()
&& !visualItem->isStandaloneCoordinate() ) {
if ( visualItem->isSimpleItem() ) {
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
if (simpleItem->specifiesAltitude()) {
altitude = simpleItem->altitude()->rawValue().toDouble();
altitudeMode = simpleItem->altitudeMode();
return true;
}
}
}
}
return false;
}
bool WaypointManager::Utils::updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber)
{
using namespace WaypointManager::Utils;
if ( list.count() < 1)
return false;
// Setup ascending sequence numbers for all visual items.
VisualMissionItem* startItem = list.value<VisualMissionItem*>(0);
assert(startItem != nullptr); // list empty?
startItem->setSequenceNumber(firstSeqNumber);
int sequenceNumber = startItem->lastSequenceNumber() + 1;
for (int i=1; i < list.count(); ++i) {
VisualMissionItem* item = list.value<VisualMissionItem*>(i);
item->setSequenceNumber(sequenceNumber);
sequenceNumber = item->lastSequenceNumber() + 1;
}
return true;
}
bool WaypointManager::Utils::updateHirarchy(QmlObjectListModel &list)
{
if ( list.count() < 1)
return false;
VisualMissionItem* currentParentItem = list.value<VisualMissionItem*>(0);
currentParentItem->childItems()->clear();
for (size_t i = 1; i < size_t( list.count() ); ++i) {
VisualMissionItem* item = list.value<VisualMissionItem*>(i);
if (item->specifiesCoordinate()) {
item->childItems()->clear();
currentParentItem = item;
} else if (item->isSimpleItem()) {
currentParentItem->childItems()->append(item);
}
}
return true;
}
bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list)
{
MissionSettingsItem *settingsItem = list.value<MissionSettingsItem *>(0);
assert(settingsItem); // list not initialized?
// Set the home position to be a delta from first coordinate.
for (int i = 1; i < list.count(); ++i) {
VisualMissionItem* item = list.value<VisualMissionItem*>(i);
assert(item);
if (item->specifiesCoordinate() && item->coordinate().isValid()) {
QGeoCoordinate c = item->coordinate().atDistanceAndAzimuth(3, 0);
c.setAltitude(0);
settingsItem->setCoordinate(c);
return true;
}
}
return false;
}
void WaypointManager::Utils::initialize(QmlObjectListModel &list,
PlanMasterController *masterController,
bool flyView)
{
MissionSettingsItem* settingsItem = new MissionSettingsItem(masterController, flyView, &list);
list.insert(0, settingsItem);
}
bool WaypointManager::Utils::updateList(QmlObjectListModel &list)
{
using namespace WaypointManager::Utils;
bool ret = updateSequenceNumbers(list);
ret = updateHirarchy(list) == false ? false : ret;
(void)updateHomePosition(list);
return ret;
}
void WaypointManager::Utils::printList(QmlObjectListModel &list)
{
for (int i = 0; i < list.count(); ++i) {
auto item = list.value<VisualMissionItem*>(i);
qWarning() << "list index: " << i << "\n"
<< "coordinate: " << item->coordinate() << "\n"
<< "seq: " << item->sequenceNumber() << "\n"
<< "last seq: " << item->lastSequenceNumber() << "\n"
<< "is simple item: " << item->isSimpleItem() << "\n\n";
}
}
void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
{
assert(speed > 0);
auto c = item->coordinate();
item->setCommand(MAV_CMD_DO_CHANGE_SPEED);
// Set coordinate must be after setCommand (setCommand sets coordinate to zero).
item->setCoordinate(c);
item->missionItem().setParam2(speed);
}
src/Wima/Utils.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "QmlObjectListModel.h"
#include "SimpleMissionItem.h"
#include <QVariant>
class PlanMasterController;
class QGeoCoordinate;
namespace WaypointManager {
namespace Utils {
template <class CoordinateType>
CoordinateType getCoordinate(const SimpleMissionItem *item) {
return item->coordinate();
}
template <> QVariant getCoordinate(const SimpleMissionItem *item) {
return QVariant::fromValue(item->coordinate());
}
/// extracts the coordinates stored in missionItems (list of MissionItems) and
/// stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool extract(const QmlObjectListModel &missionItems,
ContainerType<CoordinateType> &coordinateList,
std::size_t startIndex, std::size_t endIndex) {
if (startIndex < std::size_t(missionItems.count()) &&
endIndex < std::size_t(missionItems.count()) &&
missionItems.count() > 0) {
if (startIndex > endIndex) {
if (!extract(missionItems, coordinateList, startIndex,
std::size_t(missionItems.count() - 1)) /*recursion*/)
return false;
if (!extract(missionItems, coordinateList, 0, endIndex) /*recursion*/)
return false;
} else {
for (std::size_t i = startIndex; i <= endIndex; ++i) {
const QObject *obj = missionItems[int(i)];
auto *mItem = qobject_cast<const SimpleMissionItem *>(obj);
if (mItem == nullptr) {
coordinateList.clear();
return false;
}
coordinateList.append(getCoordinate<CoordinateType>(mItem));
}
}
} else
return false;
return true;
}
/// extracts the coordinates stored in missionItems (list of MissionItems) and
/// stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool extract(const QmlObjectListModel &missionItems,
ContainerType<CoordinateType> &coordinateList) {
return extract(missionItems, coordinateList, std::size_t(0),
std::size_t(missionItems.count()));
}
/// extracts the coordinates stored in missionItems (list of MissionItems) and
/// stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool extract(const ContainerType<CoordinateType> &source,
ContainerType<CoordinateType> &destination, std::size_t startIndex,
std::size_t endIndex) {
if (startIndex < std::size_t(source.size()) &&
endIndex < std::size_t(source.size()) && source.size() > 0) {
if (startIndex > endIndex) {
if (!extract(source, destination, startIndex,
std::size_t(long(source.size()) - 1)) /*recursion*/)
return false;
if (!extract(source, destination, 0, endIndex) /*recursion*/)
return false;
} else {
for (std::size_t i = startIndex; i <= endIndex; ++i) {
destination.append(source[i]);
}
}
} else
return false;
return true;
}
//!
//! \brief Makes the SimpleMissionItem \p item a takeoff command.
//! \param item SimpleMissionItem
//! \return Returns true if successfull, false either.
//!
bool makeTakeOffCmd(SimpleMissionItem *item,
PlanMasterController *masterController);
//!
//! \brief Makes the SimpleMissionItem \p item a land command.
//! \param item SimpleMissionItem
//! \return Returns true if successfull, false either.
//!
bool makeLandCmd(SimpleMissionItem *item,
PlanMasterController *masterController);
//!
//! \brief Makes the SimpleMissionItem \p item a MAV_CMD_DO_CHANGE_SPEED item.
//! \param item SimpleMissionItem.
//! \param speed Speed (must be greater zero).
//!
void makeSpeedCmd(SimpleMissionItem *item, double speed);
//!
//! \brief Initializes the list \p list.
//!
//! Adds a MissionSettingsItem to \p list.
//! \param list List of VisualMissionItems.
//! \param flyView Fly- or planview.
void initialize(QmlObjectListModel &list,
PlanMasterController *masterController, bool flyView = true);
//! @brief Creates (from QGeoCoordinate) and inserts a SimpleMissionItem at the
//! given index to list. \param coordinate Coordinate of the SimpleMissionItem.
//! \param index Index to insert SimpleMissionItem.
//! \param list List of SimpleMissionItems.
//! \param flyView Fly- or planview.
//! \param parent Parent of the SimpleMissionItem (Set to &list if nullptr).
//! \param doUpdates Neccessary update are performed to the list \p list if set
//! to true. Set this to true for the last item only, if inserting multiple
//! items. \return Returns true on success, false either.
//!
//! @note The list must be initialized. (\see \fn initialize)
//! @note This function doesn't establish any signal-slot connections (\see \fn
//! MissionController::_initVisualItem).
//! @note This function doesn't set the MissionFlightStatus_t (\see \fn
//! VisualMissionItem::setMissionFlightStatus).
bool insertMissionItem(const QGeoCoordinate &coordinate, long index,
QmlObjectListModel &list,
PlanMasterController *masterController,
bool flyView = true, QObject *parent = nullptr,
bool doUpdates = true);
//!
//! \brief Performs a list update.
//!
//! Updates the home position of the MissionSettingsItem, the parent child
//! hirarchy and the sequence numbers. \param list List containing
//! VisualMissionItems. \return Returns true if no errors occured, false either.
bool updateList(QmlObjectListModel &list);
//! @brief Updates the sequence numbers of the VisualMissionItems inside \p
//! list.
//!
//! \param list List containing VisualMissionItems.
//! \param firstSeqNumber The sequence number of the fisrt list entry (defaul =
//! 0). \return Returns true if successfull, false either.
//!
//! \note If the list \p list contains only one VisualMissionItem it's sequence
//! number is set to 0.
bool updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber = 0);
//! @brief Update the parent child item hierarchy of the VisualMissionItems
//! inside \p list.
//!
//! \param list List containing VisualMissionItems.
//! \return Returns true if successfull, false either.
//!
//! \note Returns true on success, false either.
bool updateHirarchy(QmlObjectListModel &list);
//! @brief Updates the home position to the first valid coordinate of the
//! VisualMissionItems inside \p list.
//!
//! \param list List containing VisualMissionItems.
//! \return Returns true if the home position was updated, false either.
bool updateHomePosition(QmlObjectListModel &list);
//!
//! \brief Prints the \p list.
//! \param list List containing VisualMissionItems.
void printList(QmlObjectListModel &list);
namespace detail {
size_t nextSequenceNumber(QmlObjectListModel &list);
bool previousAltitude(QmlObjectListModel &list, size_t index, double &altitude,
int &altitudeMode);
} // namespace detail
} // namespace Utils
} // namespace WaypointManager
src/Wima/WimaBridge.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaBridge.h"
#include "GenericSingelton.h"
WimaBridge::WimaBridge(QObject *parent) : QObject(parent) {}
WimaBridge *WimaBridge::createInstance() { return new WimaBridge(); }
WimaBridge::~WimaBridge() {}
WimaBridge *WimaBridge::instance() {
return GenericSingelton<WimaBridge>::instance(WimaBridge::createInstance);
}
const WimaPlanData &WimaBridge::planData() const { return this->planData_; }
const QVector<int> &WimaBridge::progress() const { return this->progress_; }
void WimaBridge::setProgress(const QVector<int> &p) {
if (this->progress_ != p) {
this->progress_ = p;
emit progressChanged();
}
}
void WimaBridge::setPlanData(const WimaPlanData &planData) {
if (this->planData_ != planData) {
this->planData_ = planData;
emit planDataChanged();
}
}
src/Wima/WimaBridge.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QObject>
#include <QSharedPointer>
#include "WimaPlanData.h"
class WimaController;
class WimaPlaner;
//!
//! \brief The WimaBridge class
//!
//! A singelton bridge establishing a link between WimaController and WimaPlaner
class WimaBridge : public QObject {
Q_OBJECT
WimaBridge(QObject *parent = nullptr);
WimaBridge(WimaBridge &other) = delete;
static WimaBridge *createInstance();
public:
~WimaBridge();
static WimaBridge *instance();
const AreaData &planData() const;
const QVector<int> &progress() const;
public slots:
void setPlanData(const AreaData &planData);
void setProgress(const QVector<int> &p);
signals:
void planDataChanged();
void progressChanged();
private:
AreaData planData_;
QVector<int> progress_;
};
src/Wima/WimaPlanData.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaPlanData.h"
AreaData::AreaData(QObject *parent) : QObject(parent) {}
AreaData::AreaData(const AreaData &other, QObject *parent)
: QObject(parent) {
*this = other;
}
AreaData &AreaData::operator=(const AreaData &other) {
this->set(other.measurementArea());
this->set(other.serviceArea());
this->set(other.joinedArea());
this->set(other.corridor());
return *this;
}
void AreaData::set(const WimaJoinedAreaData &areaData) {
if (_joinedArea != areaData) {
_joinedArea = areaData;
emit joinedAreaChanged();
}
}
void AreaData::set(const WimaServiceAreaData &areaData) {
if (_serviceArea != areaData) {
_serviceArea = areaData;
emit serviceAreaChanged();
}
}
void AreaData::set(const WimaCorridorData &areaData) {
if (_corridor != areaData) {
_corridor = areaData;
emit corridorChanged();
}
}
void AreaData::set(const WimaMeasurementAreaData &areaData) {
if (_measurementArea != areaData) {
_measurementArea = areaData;
emit measurementAreaChanged();
if (_measurementArea.coordinateList().size() > 0) {
setOrigin(_measurementArea.coordinateList().first());
} else {
setOrigin(QGeoCoordinate());
}
}
}
void AreaData::set(const WimaJoinedArea &areaData) {
if (_joinedArea != areaData) {
_joinedArea = areaData;
emit joinedAreaChanged();
}
}
void AreaData::set(const WimaServiceArea &areaData) {
if (_serviceArea != areaData) {
_serviceArea = areaData;
emit serviceAreaChanged();
}
}
void AreaData::set(const WimaCorridor &areaData) {
if (_corridor != areaData) {
_corridor = areaData;
emit corridorChanged();
}
}
void AreaData::set(const WimaMeasurementArea &areaData) {
if (_measurementArea != areaData) {
_measurementArea = areaData;
emit measurementAreaChanged();
if (_measurementArea.coordinateList().size() > 0) {
setOrigin(_measurementArea.coordinateList().first());
} else {
setOrigin(QGeoCoordinate());
}
}
}
void AreaData::clear() { *this = AreaData(); }
QGeoCoordinate AreaData::origin() { return _origin; }
bool AreaData::isValid() {
return _measurementArea.coordinateList().size() >= 3 &&
_serviceArea.coordinateList().size() >= 3 && _origin.isValid();
}
const WimaJoinedAreaData &AreaData::joinedArea() const {
return this->_joinedArea;
}
const WimaServiceAreaData &AreaData::serviceArea() const {
return this->_serviceArea;
}
const WimaCorridorData &AreaData::corridor() const {
return this->_corridor;
}
const WimaMeasurementAreaData &AreaData::measurementArea() const {
return this->_measurementArea;
}
WimaJoinedAreaData &AreaData::joinedArea() { return this->_joinedArea; }
WimaServiceAreaData &AreaData::serviceArea() { return this->_serviceArea; }
WimaCorridorData &AreaData::corridor() { return this->_corridor; }
WimaMeasurementAreaData &AreaData::measurementArea() {
return this->_measurementArea;
}
bool AreaData::operator==(const AreaData &other) const {
return this->_joinedArea == other._joinedArea &&
this->_measurementArea == other._measurementArea &&
this->_corridor == other._corridor &&
this->_serviceArea == other._serviceArea;
}
bool AreaData::operator!=(const AreaData &other) const {
return !(*this == other);
}
void AreaData::setOrigin(const QGeoCoordinate &origin) {
if (this->_origin != origin) {
this->_origin = origin;
emit originChanged();
}
}
src/Wima/WimaPlanData.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QGeoCoordinate>
#include <QObject>
#include "Geometry/WimaAreaData.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaServiceAreaData.h"
#include "MissionItem.h"
class WimaPlanData : public QObject {
Q_OBJECT
public:
WimaPlanData(QObject *parent = nullptr);
WimaPlanData(const WimaPlanData &other, QObject *parent = nullptr);
WimaPlanData &operator=(const WimaPlanData &other);
// Member Methodes
void set(const WimaJoinedAreaData &areaData);
void set(const WimaServiceAreaData &areaData);
void set(const WimaCorridorData &areaData);
void set(const WimaMeasurementAreaData &areaData);
void set(const WimaJoinedArea &areaData);
void set(const WimaServiceArea &areaData);
void set(const WimaCorridor &areaData);
void set(const WimaMeasurementArea &areaData);
void clear();
const WimaJoinedAreaData &joinedArea() const;
const WimaServiceAreaData &serviceArea() const;
const WimaCorridorData &corridor() const;
const WimaMeasurementAreaData &measurementArea() const;
WimaJoinedAreaData &joinedArea();
WimaServiceAreaData &serviceArea();
WimaCorridorData &corridor();
WimaMeasurementAreaData &measurementArea();
QGeoCoordinate origin();
bool isValid();
bool operator==(const WimaPlanData &other) const;
bool operator!=(const WimaPlanData &other) const;
signals:
void joinedAreaChanged();
void serviceAreaChanged();
void corridorChanged();
void measurementAreaChanged();
void originChanged();
private:
void setOrigin(const QGeoCoordinate &origin);
WimaJoinedAreaData _joinedArea;
WimaServiceAreaData _serviceArea;
WimaCorridorData _corridor;
WimaMeasurementAreaData _measurementArea;
QGeoCoordinate _origin;
};
src/Wima/WimaPlaner.cc deleted 100644 → 0
View file @ 458ab210
#include "WimaPlaner.h"
#include "MissionController.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.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"
#include "WimaStateMachine.h"
using namespace wima_planer_detail;
#include <functional>
QGC_LOGGING_CATEGORY(WimaPlanerLog, "WimaPlanerLog")
class CommandRAII {
std::function<void(void)> f;
public:
CommandRAII(const std::function<void(void)> &fun) : f(fun) {}
~CommandRAII() { f(); }
};
const char *WimaPlaner::wimaFileExtension = "wima";
const char *WimaPlaner::areaItemsName = "AreaItems";
const char *WimaPlaner::missionItemsName = "MissionItems";
WimaPlaner::WimaPlaner(QObject *parent)
: QObject(parent), _masterController(nullptr), _missionController(nullptr),
_currentAreaIndex(-1), _joinedArea(this), _survey(nullptr),
_nemoInterface(this), _stateMachine(new WimaStateMachine),
_areasMonitored(false), _missionControllerMonitored(false),
_progressLocked(false), _synchronized(false) {
connect(this, &WimaPlaner::currentPolygonIndexChanged, this,
&WimaPlaner::updatePolygonInteractivity);
// Monitoring.
enableAreaMonitoring();
// Mission controller not set at this point. Not enabling monitoring.
#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);
#endif
// NemoInterface
connect(&this->_nemoInterface, &NemoInterface::progressChanged, this,
&WimaPlaner::nemoInterfaceProgressChangedHandler);
// StateMachine
connect(this->_stateMachine.get(), &WimaStateMachine::upToDateChanged, this,
&WimaPlaner::needsUpdateChanged);
connect(this->_stateMachine.get(), &WimaStateMachine::surveyReadyChanged, this,
&WimaPlaner::readyForSynchronizationChanged);
connect(this->_stateMachine.get(), &WimaStateMachine::surveyReadyChanged, this,
&WimaPlaner::surveyReadyChanged);
}
WimaPlaner::~WimaPlaner() {}
PlanMasterController *WimaPlaner::masterController() {
return _masterController;
}
MissionController *WimaPlaner::missionController() {
return _missionController;
}
QmlObjectListModel *WimaPlaner::visualItems() { return &_visualItems; }
int WimaPlaner::currentPolygonIndex() const { return _currentAreaIndex; }
QString WimaPlaner::currentFile() const { return _currentFile; }
QStringList WimaPlaner::loadNameFilters() const {
QStringList filters;
filters << tr("Supported types (*.%1 *.%2)")
.arg(wimaFileExtension)
.arg(AppSettings::planFileExtension)
<< tr("All Files (*.*)");
return filters;
}
QStringList WimaPlaner::saveNameFilters() const {
QStringList filters;
filters << tr("Supported types (*.%1 *.%2)")
.arg(wimaFileExtension)
.arg(AppSettings::planFileExtension);
return filters;
}
QString WimaPlaner::fileExtension() const { return wimaFileExtension; }
QGeoCoordinate WimaPlaner::joinedAreaCenter() const {
return _joinedArea.center();
}
NemoInterface *WimaPlaner::nemoInterface() { return &_nemoInterface; }
void WimaPlaner::setMasterController(PlanMasterController *masterC) {
if (_masterController != masterC) {
_masterController = masterC;
emit masterControllerChanged();
}
}
void WimaPlaner::setMissionController(MissionController *missionC) {
if (_missionController != missionC) {
disableMissionControllerMonitoring();
_missionController = missionC;
enableMissionControllerMonitoring();
emit missionControllerChanged();
}
}
void WimaPlaner::setCurrentPolygonIndex(int index) {
if (index >= 0 && index < _visualItems.count() &&
index != _currentAreaIndex) {
_currentAreaIndex = index;
emit currentPolygonIndexChanged(index);
}
}
void WimaPlaner::setProgressLocked(bool l) {
if (this->_progressLocked != l) {
this->_progressLocked = l;
emit progressLockedChanged();
if (!this->_progressLocked) {
if (this->_measurementArea.setProgress(this->_nemoInterface.progress()))
this->_update();
}
}
}
bool WimaPlaner::synchronized() { return _synchronized; }
bool WimaPlaner::needsUpdate() { return !this->_stateMachine->upToDate(); }
bool WimaPlaner::readyForSynchronization() {
return this->_stateMachine->surveyReady();
}
bool WimaPlaner::surveyReady() { return this->_stateMachine->surveyReady(); }
bool WimaPlaner::progressLocked() { return this->_progressLocked; }
void WimaPlaner::removeArea(int index) {
if (index >= 0 && index < _visualItems.count()) {
WimaArea *area = qobject_cast<WimaArea *>(_visualItems.removeAt(index));
if (area == nullptr) {
qCWarning(WimaPlanerLog)
<< "removeArea(): nullptr catched, internal error.";
return;
}
area->clear();
area->borderPolygon()->clear();
emit visualItemsChanged();
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;
}
if (_currentAreaIndex >= _visualItems.count()) {
setCurrentPolygonIndex(_visualItems.count() - 1);
} else {
updatePolygonInteractivity(_currentAreaIndex);
}
} else {
qCWarning(WimaPlanerLog) << "removeArea(): Index out of bounds!";
}
}
bool WimaPlaner::addMeasurementArea() {
if (!_visualItems.contains(&_measurementArea)) {
_visualItems.append(&_measurementArea);
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
return true;
} else {
return false;
}
}
bool WimaPlaner::addServiceArea() {
if (!_visualItems.contains(&_serviceArea)) {
_visualItems.append(&_serviceArea);
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
return true;
} else {
return false;
}
}
bool WimaPlaner::addCorridor() {
if (!_visualItems.contains(&_corridor)) {
_visualItems.append(&_corridor);
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
return true;
} else {
return false;
}
}
void WimaPlaner::removeAll() {
bool changesApplied = false;
// Delete Pointers.
while (_visualItems.count() > 0) {
removeArea(0);
changesApplied = true;
}
_measurementArea = WimaMeasurementArea();
_joinedArea = WimaJoinedArea();
_serviceArea = WimaServiceArea();
_corridor = WimaCorridor();
// Remove missions items.
_missionController->removeAll();
_currentFile = "";
_survey = nullptr;
emit currentFileChanged();
if (changesApplied)
emit visualItemsChanged();
}
void WimaPlaner::update() { this->_update(); }
void WimaPlaner::_update() {
setSynchronized(false);
switch (this->_stateMachine->state()) {
case STATE::NEEDS_INIT: {
if (this->_measurementArea.ready()) {
this->_stateMachine->updateState(EVENT::INIT_DONE);
this->_update();
} else {
this->_stateMachine->updateState(EVENT::M_AREA_NOT_READY);
}
} break;
case STATE::WAITING_FOR_TILE_UPDATE: {
} break;
case STATE::NEEDS_J_AREA_UPDATE: {
// check if at least service area and measurement area are available
if (_visualItems.indexOf(&_serviceArea) == -1 ||
_visualItems.indexOf(&_measurementArea) == -1)
return;
// Check if polygons have at least three vertices
if (_serviceArea.count() < 3) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Service area has less than three vertices."));
return;
}
if (_measurementArea.count() < 3) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"),
tr("Measurement area has less than three vertices."));
return;
}
// Check for simple polygons
if (!_serviceArea.isSimplePolygon()) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Service area is not a simple polygon. Only "
"simple polygons allowed."));
return;
}
if (!_corridor.isSimplePolygon() && _corridor.count() > 0) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Corridor is not a simple polygon. Only simple "
"polygons allowed."));
return;
}
if (!_measurementArea.isSimplePolygon()) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Measurement area is not a simple polygon. Only "
"simple polygons allowed."));
return;
}
if (!_serviceArea.containsCoordinate(_serviceArea.depot())) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Depot not inside service area."));
return;
}
// Join areas.
_joinedArea.setPath(_serviceArea.path());
if (_corridor.count() >= 3) {
_joinedArea.join(_corridor);
}
if (!_joinedArea.join(_measurementArea)) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"),
tr("Not able to join areas. Service and measurement area"
" must be overlapping, or connected through a "
"corridor."));
return;
}
this->_stateMachine->updateState(EVENT::J_AREA_UPDATED);
this->_update();
} break; // STATE::NEEDS_J_AREA_UPDATE
case STATE::NEEDS_SURVEY_UPDATE: {
// Need to insert Survey?
QmlObjectListModel *missionItems = _missionController->visualItems();
int surveyIndex = missionItems->indexOf(_survey);
// Create survey item if not yet present.
if (surveyIndex < 0) {
_missionController->insertComplexMissionItem(
CircularSurvey::name,
_measurementArea.center(), missionItems->count());
_survey = qobject_cast<CircularSurvey *>(
missionItems->get(missionItems->count() - 1));
if (_survey == nullptr) {
qCWarning(WimaPlanerLog) << "_survey == nullptr";
return;
}
// establish connections
connect(_survey, &CircularSurvey::visualTransectPointsChanged, this,
&WimaPlaner::CSVisualTransectPointsChangedHandler);
connect(_survey, &CircularSurvey::destroyed, this,
&WimaPlaner::CSDestroyedHandler);
}
(void)toPlanData(this->_survey->planData());
} break; // STATE::NEEDS_SURVEY_UPDATE
case STATE::WAITING_FOR_SURVEY_UPDATE: {
} break;
case STATE::NEEDS_PATH_UPDATE: {
// Check if survey is present.
QmlObjectListModel *missionItems = _missionController->visualItems();
int surveyIndex = missionItems->indexOf(_survey);
if (surveyIndex < 0) {
this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED);
this->_update();
} else {
// Remove old arrival and return path.
int size = missionItems->count();
for (int i = surveyIndex + 1; i < size; i++)
_missionController->removeVisualItem(surveyIndex + 1);
for (int i = surveyIndex - 1; i > 1; i--)
_missionController->removeVisualItem(i);
// set home position to serArea center
MissionSettingsItem *settingsItem =
qobject_cast<MissionSettingsItem *>(missionItems->get(0));
if (settingsItem == nullptr) {
qCWarning(WimaPlanerLog) << "update(): settingsItem == nullptr";
return;
}
// set altitudes
auto depot = _serviceArea.depot();
depot.setAltitude(0);
settingsItem->setCoordinate(depot);
// set takeoff position
_missionController->insertTakeoffItem(depot, 1, false);
if (_survey->visualTransectPoints().size() == 0) {
qCWarning(WimaPlanerLog) << "update(): survey no points";
return;
}
// calculate path from take off to survey
QGeoCoordinate start = depot;
QGeoCoordinate end = _survey->coordinate();
QVector<QGeoCoordinate> path;
if (!shortestPath(start, end, path)) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Path Error"), tr("Not able to calculate path from "
"depot position to measurement area. Please "
"double check area and route parameters."));
return;
}
for (int i = 1; i < path.count() - 1; i++) {
(void)_missionController->insertSimpleMissionItem(
path[i], missionItems->count() - 1);
}
// calculate return path
start = _survey->exitCoordinate();
end = depot;
path.clear();
if (!shortestPath(start, end, path)) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Path Error"), tr("Not able to calculate path from "
"measurement area to depot position. Please "
"double check area and route parameters."));
return;
}
for (int i = 1; i < path.count() - 1; i++) {
(void)_missionController->insertSimpleMissionItem(
path[i], missionItems->count());
}
// Add waypoint (rover ignores land command).
(void)_missionController->insertSimpleMissionItem(depot,
missionItems->count());
// create land position item
(void)_missionController->insertLandItem(depot, missionItems->count(),
true);
auto surveyIndex = _missionController->visualItems()->indexOf(_survey);
_missionController->setCurrentPlanViewSeqNum(surveyIndex, true);
this->_stateMachine->updateState(EVENT::PATH_UPDATED);
}
} break; // STATE::NEEDS_PATH_UPDATE
case STATE::UP_TO_DATE: {
} break; // STATE::UP_TO_DATE
} // switch
}
void WimaPlaner::CSDestroyedHandler() {
this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED);
}
void WimaPlaner::CSVisualTransectPointsChangedHandler() {
if (this->_survey && this->_survey->calculating()){
this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED);
} else {
this->_stateMachine->updateState(EVENT::SURVEY_UPDATED);
this->_update();
}
}
void WimaPlaner::mAreaPathChangedHandler() {
this->_stateMachine->updateState(EVENT::M_AREA_PATH_CHANGED);
}
void WimaPlaner::mAreaTilesChangedHandler() {
this->_nemoInterface.setTileData(this->_measurementArea.tileData());
this->_stateMachine->updateState(EVENT::M_AREA_TILES_CHANGED);
}
void WimaPlaner::mAreaProgressChangedHandler() {
this->_stateMachine->updateState(EVENT::M_AREA_PROGRESS_CHANGED);
}
void WimaPlaner::mAreaProgressAcceptedHandler() { this->_update(); }
void WimaPlaner::mAreaReadyChangedHandler() {
if (this->_measurementArea.ready()) {
this->_stateMachine->updateState(EVENT::M_AREA_READY);
} else {
this->_stateMachine->updateState(EVENT::M_AREA_NOT_READY);
}
}
void WimaPlaner::sAreaPathChangedHandler() {
this->_stateMachine->updateState(EVENT::S_AREA_PATH_CHANGED);
}
void WimaPlaner::corridorPathChangedHandler() {
this->_stateMachine->updateState(EVENT::CORRIDOR_PATH_CHANGED);
}
void WimaPlaner::depotChangedHandler() {
this->_stateMachine->updateState(EVENT::DEPOT_CHANGED);
}
void WimaPlaner::missionControllerVisualItemsChangedHandler() {
// Search for survey.
auto surveyIndex = _missionController->visualItems()->indexOf(_survey);
if (surveyIndex < 0) {
// survey not found.
this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED);
} else {
this->_stateMachine->updateState(EVENT::PATH_CHANGED);
}
}
void WimaPlaner::missionControllerWaypointPathChangedHandler() {
missionControllerVisualItemsChangedHandler();
}
void WimaPlaner::missionControllerNewItemsFromVehicleHandler() {
this->_stateMachine->updateState(EVENT::MISSION_ITEMS_DESTROYED);
}
void WimaPlaner::missionControllerMissionItemCountChangedHandler() {
missionControllerVisualItemsChangedHandler();
}
void WimaPlaner::nemoInterfaceProgressChangedHandler() {
auto p = this->_nemoInterface.progress();
WimaBridge::instance()->setProgress(p);
if (!progressLocked()) {
this->_measurementArea.setProgress(p);
this->_update();
}
}
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()->warningMessageBoxOnMainThread(
tr("Save Error"),
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 {
if (planFilename.contains(".")) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Save Error"), tr("File format not supported"));
} else {
qgcApp()->warningMessageBoxOnMainThread(
tr("Save Error"), tr("File without file extension not accepted."));
return;
}
}
QJsonDocument saveDoc = saveToJson(fileType);
file.write(saveDoc.toJson());
if (_currentFile != planFilename) {
_currentFile = planFilename;
emit currentFileChanged();
}
}
}
bool WimaPlaner::loadFromCurrent() { return loadFromFile(_currentFile); }
bool WimaPlaner::loadFromFile(const QString &filename) {
// Remove obsolete connections.
disableAreaMonitoring();
disableMissionControllerMonitoring();
CommandRAII onExit([this] {
this->enableAreaMonitoring();
this->enableMissionControllerMonitoring();
});
// disconnect old survey
if (_survey != nullptr) {
disconnect(_survey, &CircularSurvey::visualTransectPointsChanged, this,
&WimaPlaner::CSVisualTransectPointsChangedHandler);
disconnect(_survey, &CircularSurvey::destroyed, this,
&WimaPlaner::CSDestroyedHandler);
}
setSynchronized(false);
// Precondition.
QString errorString;
QString errorMessage =
tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");
if (filename.isEmpty()) {
return false;
}
QFileInfo fileInfo(filename);
QFile file(filename);
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
errorString = file.errorString() + QStringLiteral(" ") + filename;
qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"),
errorMessage.arg(errorString));
return false;
}
if (fileInfo.suffix() == wimaFileExtension) {
QJsonDocument jsonDoc;
QByteArray bytes = file.readAll();
if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) {
qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"),
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()->warningMessageBoxOnMainThread(
tr("Load Error"), errorMessage.arg(errorString));
return false;
}
validAreaCounter++;
_visualItems.append(&_measurementArea);
emit visualItemsChanged();
} else if (jsonArea[WimaArea::areaTypeName] ==
WimaServiceArea::wimaServiceAreaName) {
bool success = _serviceArea.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Load Error"), errorMessage.arg(errorString));
return false;
}
validAreaCounter++;
_visualItems.append(&_serviceArea);
emit visualItemsChanged();
} else if (jsonArea[WimaArea::areaTypeName] ==
WimaCorridor::WimaCorridorName) {
bool success = _corridor.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->warningMessageBoxOnMainThread(
tr("Load Error"), errorMessage.arg(errorString));
return false;
}
validAreaCounter++;
_visualItems.append(&_corridor);
emit visualItemsChanged();
} else {
errorString +=
QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName));
qgcApp()->warningMessageBoxOnMainThread(
tr("Load Error"), 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();
QJsonDocument missionJsonDoc = QJsonDocument(missionObject);
// create temporary file with missionItems
QFile temporaryFile;
QString cropedFileName = filename.section("/", 0, -2);
QString temporaryFileName;
for (int i = 0;; i++) {
temporaryFileName =
cropedFileName +
QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);
if (!QFile::exists(temporaryFileName)) {
temporaryFile.setFileName(temporaryFileName);
if (temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text)) {
break;
}
}
if (i > 1000) {
qCWarning(WimaPlanerLog)
<< "loadFromFile(): not able to create temporary file.";
return false;
}
}
temporaryFile.write(missionJsonDoc.toJson());
temporaryFile.close();
// load from temporary file
_masterController->loadFromFile(temporaryFileName);
QmlObjectListModel *missionItems = _missionController->visualItems();
_survey = nullptr;
for (int i = 0; i < missionItems->count(); i++) {
_survey = missionItems->value<CircularSurvey *>(i);
if (_survey != nullptr) {
connect(_survey, &CircularSurvey::visualTransectPointsChanged, this,
&WimaPlaner::CSVisualTransectPointsChangedHandler);
connect(_survey, &CircularSurvey::destroyed, this,
&WimaPlaner::CSDestroyedHandler);
break;
}
}
// remove temporary file
if (!temporaryFile.remove()) {
qCWarning(WimaPlanerLog)
<< "WimaPlaner::loadFromFile(): not able to remove "
"temporary file.";
}
return true;
} else {
errorString += QString(tr("File extension not supported.\n"));
qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"),
errorMessage.arg(errorString));
return false;
}
}
void WimaPlaner::updatePolygonInteractivity(int index) {
if (index >= 0 && index < _visualItems.count()) {
resetAllInteractive();
WimaArea *interactivePoly =
qobject_cast<WimaArea *>(_visualItems.get(index));
if (interactivePoly != nullptr)
interactivePoly->setWimaAreaInteractive(true);
}
}
void WimaPlaner::synchronize() {
if (readyForSynchronization()) {
AreaData planData;
if (toPlanData(planData)) {
WimaBridge::instance()->setPlanData(planData);
setSynchronized(true);
} else {
qCWarning(WimaPlanerLog) << "error creating plan data.";
}
}
}
bool WimaPlaner::shortestPath(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::setSynchronized(bool s) {
if (this->_synchronized != s) {
this->_synchronized = s;
emit this->synchronizedChanged();
}
}
void WimaPlaner::enableAreaMonitoring() {
if (!areasMonitored()) {
connect(&this->_measurementArea, &WimaArea::pathChanged, this,
&WimaPlaner::mAreaPathChangedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, this,
&WimaPlaner::mAreaTilesChangedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::progressChanged,
this, &WimaPlaner::mAreaProgressChangedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted,
this, &WimaPlaner::mAreaProgressAcceptedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::readyChanged, this,
&WimaPlaner::mAreaReadyChangedHandler);
connect(&this->_serviceArea, &WimaArea::pathChanged, this,
&WimaPlaner::sAreaPathChangedHandler);
connect(&this->_serviceArea, &WimaServiceArea::depotChanged, this,
&WimaPlaner::depotChangedHandler);
connect(&this->_corridor, &WimaArea::pathChanged, this,
&WimaPlaner::corridorPathChangedHandler);
this->_areasMonitored = true;
}
}
void WimaPlaner::disableAreaMonitoring() {
if (areasMonitored()) {
disconnect(&this->_measurementArea, &WimaArea::pathChanged, this,
&WimaPlaner::mAreaPathChangedHandler);
disconnect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged,
this, &WimaPlaner::mAreaTilesChangedHandler);
disconnect(&this->_measurementArea, &WimaMeasurementArea::progressChanged,
this, &WimaPlaner::mAreaProgressChangedHandler);
disconnect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted,
this, &WimaPlaner::mAreaProgressAcceptedHandler);
disconnect(&this->_measurementArea, &WimaMeasurementArea::readyChanged,
this, &WimaPlaner::mAreaReadyChangedHandler);
disconnect(&this->_serviceArea, &WimaArea::pathChanged, this,
&WimaPlaner::sAreaPathChangedHandler);
disconnect(&this->_serviceArea, &WimaServiceArea::depotChanged, this,
&WimaPlaner::depotChangedHandler);
disconnect(&this->_corridor, &WimaArea::pathChanged, this,
&WimaPlaner::corridorPathChangedHandler);
this->_areasMonitored = false;
}
}
void WimaPlaner::enableMissionControllerMonitoring() {
if (!missionControllerMonitored() && this->missionController() != nullptr) {
connect(this->missionController(), &MissionController::visualItemsChanged,
this, &WimaPlaner::missionControllerVisualItemsChangedHandler);
connect(this->missionController(), &MissionController::waypointPathChanged,
this, &WimaPlaner::missionControllerWaypointPathChangedHandler);
connect(this->missionController(), &MissionController::newItemsFromVehicle,
this, &WimaPlaner::missionControllerNewItemsFromVehicleHandler);
connect(this->missionController(),
&MissionController::missionItemCountChanged, this,
&WimaPlaner::missionControllerMissionItemCountChangedHandler);
this->_missionControllerMonitored = true;
}
}
void WimaPlaner::disableMissionControllerMonitoring() {
if (missionControllerMonitored() && this->missionController() != nullptr) {
disconnect(this->missionController(),
&MissionController::visualItemsChanged, this,
&WimaPlaner::missionControllerVisualItemsChangedHandler);
disconnect(this->missionController(),
&MissionController::waypointPathChanged, this,
&WimaPlaner::missionControllerWaypointPathChangedHandler);
disconnect(this->missionController(),
&MissionController::newItemsFromVehicle, this,
&WimaPlaner::missionControllerNewItemsFromVehicleHandler);
disconnect(this->missionController(),
&MissionController::missionItemCountChanged, this,
&WimaPlaner::missionControllerMissionItemCountChangedHandler);
this->_missionControllerMonitored = false;
}
}
bool WimaPlaner::areasMonitored() { return this->_areasMonitored; }
bool WimaPlaner::missionControllerMonitored() {
return this->_missionControllerMonitored;
}
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() {
updatePolygonInteractivity(_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 update() was
* successful.
*
* \sa WimaController, WimaPlanData
*/
bool WimaPlaner::toPlanData(AreaData &planData) {
planData.append(_measurementArea);
planData.append(_serviceArea);
planData.append(_corridor);
planData.append(_joinedArea);
return planData.isValid();
}
#ifndef NDEBUG
void WimaPlaner::autoLoadMission() {
loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/"
"KlingenbachTest.wima");
synchronize();
}
#endif
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) {
qCWarning(WimaPlanerLog) << "saveing, 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);
}
json[areaItemsName] = jsonArray;
json[missionItemsName] = _masterController->saveToJson().object();
return QJsonDocument(json);
} else if (fileType == FileType::PlanFile) {
return _masterController->saveToJson();
}
return QJsonDocument(json);
}
src/Wima/WimaPlaner.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include "QmlObjectListModel.h"
#include <QObject>
#include <QScopedPointer>
#include <QSharedPointer>
#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 "Snake/NemoInterface.h"
#include "JsonHelper.h"
class MissionController;
class PlanMasterController;
namespace wima_planer_detail {
class WimaStateMachine;
}
class WimaPlaner : public QObject {
Q_OBJECT
enum FileType { WimaFile, PlanFile };
public:
WimaPlaner(QObject *parent = nullptr);
~WimaPlaner();
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(NemoInterface *nemoInterface READ nemoInterface CONSTANT)
Q_PROPERTY(bool synchronized READ synchronized NOTIFY synchronizedChanged)
Q_PROPERTY(bool needsUpdate READ needsUpdate NOTIFY needsUpdateChanged)
Q_PROPERTY(bool readyForSynchronization READ readyForSynchronization NOTIFY
readyForSynchronizationChanged)
Q_PROPERTY(bool surveyReady READ surveyReady NOTIFY surveyReadyChanged)
Q_PROPERTY(bool progressLocked READ progressLocked WRITE setProgressLocked
NOTIFY progressLockedChanged)
// Property accessors
PlanMasterController *masterController(void);
MissionController *missionController(void);
QmlObjectListModel *visualItems(void);
int currentPolygonIndex(void) const;
QString currentFile(void) const;
QStringList loadNameFilters(void) const;
QStringList saveNameFilters(void) const;
QString fileExtension(void) const;
QGeoCoordinate joinedAreaCenter(void) const;
NemoInterface *nemoInterface(void);
bool synchronized();
bool needsUpdate();
bool readyForSynchronization();
bool surveyReady();
bool progressLocked();
// 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 setProgressLocked(bool l);
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 void update();
/// Pushes the generated mission data to the wimaController.
Q_INVOKABLE void synchronize();
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);
// 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 synchronizedChanged(void);
void needsUpdateChanged(void);
void readyForSynchronizationChanged(void);
void surveyReadyChanged(void);
void progressLockedChanged();
private slots:
void updatePolygonInteractivity(int index);
void _update();
void CSDestroyedHandler();
void CSVisualTransectPointsChangedHandler();
void mAreaPathChangedHandler();
void mAreaTilesChangedHandler();
void mAreaProgressChangedHandler();
void mAreaProgressAcceptedHandler();
void mAreaReadyChangedHandler();
void sAreaPathChangedHandler();
void corridorPathChangedHandler();
void depotChangedHandler();
void missionControllerVisualItemsChangedHandler();
void missionControllerWaypointPathChangedHandler();
void missionControllerNewItemsFromVehicleHandler();
void missionControllerMissionItemCountChangedHandler();
void nemoInterfaceProgressChangedHandler();
#ifndef NDEBUG
void autoLoadMission(void);
#endif
private:
signals:
void joinedAreaValidChanged();
void stateChanged();
private:
// Member Functions
bool toPlanData(AreaData &planData);
bool shortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
void setSynchronized(bool s);
void enableAreaMonitoring();
void disableAreaMonitoring();
void enableMissionControllerMonitoring();
void disableMissionControllerMonitoring();
bool areasMonitored();
bool missionControllerMonitored();
// Member Variables
PlanMasterController *_masterController;
MissionController *_missionController;
int _currentAreaIndex;
QString _currentFile;
WimaMeasurementArea _measurementArea;
WimaServiceArea _serviceArea;
WimaCorridor _corridor;
WimaJoinedArea _joinedArea;
QmlObjectListModel _visualItems; // all areas
CircularSurvey *_survey;
#ifndef NDEBUG
QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents
// seg. faults
#endif
NemoInterface _nemoInterface;
// State
QScopedPointer<wima_planer_detail::WimaStateMachine> _stateMachine;
bool _areasMonitored;
bool _missionControllerMonitored;
bool _progressLocked;
bool _synchronized; // true if planData is synchronized with
// wimaController
};
src/Wima/WimaStateMachine.cpp deleted 100644 → 0
View file @ 458ab210
#include "WimaStateMachine.h"
#include "QGCLoggingCategory.h"
#include <QDebug>
const QLoggingCategory &WimaPlanerLog();
namespace wima_planer_detail {
template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value);
}
WimaStateMachine::WimaStateMachine(QObject *parent)
: QObject(parent), _state(STATE::NEEDS_INIT) {}
STATE WimaStateMachine::state() { return this->_state; }
void WimaStateMachine::updateState(EVENT e) {
qCDebug(WimaPlanerLog) << "StateMachine::updateState(): event:" << e;
switch (this->_state) {
case STATE::NEEDS_INIT:
switch (e) {
case EVENT::INIT_DONE:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_NOT_READY:
case EVENT::M_AREA_READY:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::MISSION_ITEMS_DESTROYED:
case EVENT::SURVEY_UPDATE_TRIGGERED:
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_CHANGED:
case EVENT::PATH_UPDATED:
break;
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
}
break; // STATE::NEEDS_INIT
case STATE::WAITING_FOR_TILE_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_NOT_READY:
break;
case EVENT::M_AREA_READY:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::MISSION_ITEMS_DESTROYED:
case EVENT::SURVEY_UPDATE_TRIGGERED:
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_CHANGED:
case EVENT::PATH_UPDATED:
break;
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
}
break; // STATE::NEEDS_INIT
case STATE::NEEDS_J_AREA_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
break;
case EVENT::M_AREA_NOT_READY:
setState(STATE::WAITING_FOR_TILE_UPDATE);
break;
case EVENT::M_AREA_READY:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
break;
case EVENT::J_AREA_UPDATED:
setState(STATE::NEEDS_SURVEY_UPDATE);
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::MISSION_ITEMS_DESTROYED:
case EVENT::SURVEY_UPDATE_TRIGGERED:
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_CHANGED:
case EVENT::PATH_UPDATED:
break;
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
}
break; // STATE::NEEDS_J_AREA_UPDATE
case STATE::NEEDS_SURVEY_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_NOT_READY:
setState(STATE::WAITING_FOR_TILE_UPDATE);
break;
case EVENT::M_AREA_READY:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::MISSION_ITEMS_DESTROYED:
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
setState(STATE::WAITING_FOR_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_CHANGED:
case EVENT::PATH_UPDATED:
break;
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
}
break; // STATE::NEEDS_SURVEY_UPDATE
case STATE::WAITING_FOR_SURVEY_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_NOT_READY:
setState(STATE::WAITING_FOR_TILE_UPDATE);
break;
case EVENT::M_AREA_READY:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::MISSION_ITEMS_DESTROYED:
setState(STATE::NEEDS_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
break;
case EVENT::SURVEY_UPDATED:
setState(STATE::NEEDS_PATH_UPDATE);
case EVENT::PATH_CHANGED:
case EVENT::PATH_UPDATED:
break;
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
}
break; // STATE::WAYTING_FOR_SURVEY_UPDATE
case STATE::NEEDS_PATH_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_NOT_READY:
setState(STATE::WAITING_FOR_TILE_UPDATE);
break;
case EVENT::M_AREA_READY:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::MISSION_ITEMS_DESTROYED:
setState(STATE::NEEDS_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
setState(STATE::WAITING_FOR_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_CHANGED:
break;
case EVENT::PATH_UPDATED:
setState(STATE::UP_TO_DATE);
break;
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
}
break; // STATE::NEEDS_PATH_UPDATE
case STATE::UP_TO_DATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_NOT_READY:
setState(STATE::WAITING_FOR_TILE_UPDATE);
break;
case EVENT::M_AREA_READY:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::MISSION_ITEMS_DESTROYED:
setState(STATE::NEEDS_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
setState(STATE::WAITING_FOR_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_CHANGED:
setState(STATE::NEEDS_PATH_UPDATE);
break;
case EVENT::PATH_UPDATED:
break;
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
}
break; // STATE::UP_TO_DATE
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown state: " << this->_state;
Q_ASSERT(false);
}
}
bool WimaStateMachine::upToDate() { return upToDate(this->_state); }
bool WimaStateMachine::surveyReady() { return surveyReady(this->_state); }
void WimaStateMachine::setState(STATE s) {
if (this->_state != s) {
auto oldState = this->_state;
this->_state = s;
emit stateChanged();
if (upToDate(oldState) != upToDate(s)) {
emit upToDateChanged();
}
if (surveyReady(oldState) != surveyReady(s)) {
emit surveyReadyChanged();
}
qCDebug(WimaPlanerLog) << "StateMachine::setState():" << oldState << "->"
<< s;
}
}
bool WimaStateMachine::surveyReady(STATE s) {
// Using a switch to enable compiler checking of used states.
bool value = false;
switch (s) {
case STATE::NEEDS_INIT:
case STATE::WAITING_FOR_TILE_UPDATE:
case STATE::NEEDS_J_AREA_UPDATE:
case STATE::NEEDS_SURVEY_UPDATE:
case STATE::WAITING_FOR_SURVEY_UPDATE:
break;
case STATE::NEEDS_PATH_UPDATE:
case STATE::UP_TO_DATE:
value = true;
break;
}
return value;
}
bool WimaStateMachine::upToDate(STATE s) {
// Using a switch to enable compiler checking of used states.
bool value = false;
switch (s) {
case STATE::NEEDS_INIT:
case STATE::WAITING_FOR_TILE_UPDATE:
case STATE::NEEDS_J_AREA_UPDATE:
case STATE::NEEDS_SURVEY_UPDATE:
case STATE::WAITING_FOR_SURVEY_UPDATE:
case STATE::NEEDS_PATH_UPDATE:
break;
case STATE::UP_TO_DATE:
value = true;
break;
}
return value;
}
QDebug &operator<<(QDebug &ds, STATE s) {
switch (s) {
case STATE::NEEDS_INIT:
ds << "NEEDS_INIT";
break;
case STATE::WAITING_FOR_TILE_UPDATE:
ds << "WAITING_FOR_TILE_UPDATE";
break;
case STATE::NEEDS_J_AREA_UPDATE:
ds << "NEEDS_J_AREA_UPDATE";
break;
case STATE::NEEDS_SURVEY_UPDATE:
ds << "NEEDS_SURVEY_UPDATE";
break;
case STATE::WAITING_FOR_SURVEY_UPDATE:
ds << "WAITING_FOR_SURVEY_UPDATE";
break;
case STATE::NEEDS_PATH_UPDATE:
ds << "NEEDS_PATH_UPDATE";
break;
case STATE::UP_TO_DATE:
ds << "UP_TO_DATE";
break;
}
return ds;
}
QDebug &operator<<(QDebug &ds, EVENT s) {
switch (s) {
case EVENT::INIT_DONE:
ds << "INIT_DONE";
break;
case EVENT::M_AREA_NOT_READY:
ds << "M_AREA_NOT_READY";
break;
case EVENT::M_AREA_READY:
ds << "M_AREA_READY";
break;
case EVENT::M_AREA_PATH_CHANGED:
ds << "M_AREA_PATH_CHANGED";
break;
case EVENT::S_AREA_PATH_CHANGED:
ds << "S_AREA_PATH_CHANGED";
break;
case EVENT::CORRIDOR_PATH_CHANGED:
ds << "CORRIDOR_PATH_CHANGED";
break;
case EVENT::M_AREA_TILES_CHANGED:
ds << "M_AREA_TILES_CHANGED";
break;
case EVENT::M_AREA_PROGRESS_CHANGED:
ds << "M_AREA_PROGRESS_CHANGED";
break;
case EVENT::J_AREA_UPDATED:
ds << "J_AREA_UPDATED";
break;
case EVENT::DEPOT_CHANGED:
ds << "DEPOT_CHANGED";
break;
case EVENT::SURVEY_DESTROYED:
ds << "SURVEY_DESTROYED";
break;
case EVENT::MISSION_ITEMS_DESTROYED:
ds << "MISSION_ITEMS_DESTROYED";
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
ds << "SURVEY_UPDATE_TRIGGERED";
break;
case EVENT::SURVEY_UPDATED:
ds << "SURVEY_UPDATED";
break;
case EVENT::PATH_CHANGED:
ds << "PATH_CHANGED";
break;
case EVENT::PATH_UPDATED:
ds << "PATH_UPDATED";
break;
}
return ds;
}
} // namespace wima_planer_detail
src/Wima/WimaStateMachine.h deleted 100644 → 0
View file @ 458ab210
#pragma once
#include <QDataStream>
#include <QObject>
namespace wima_planer_detail {
enum class STATE {
NEEDS_INIT,
WAITING_FOR_TILE_UPDATE,
NEEDS_J_AREA_UPDATE,
NEEDS_SURVEY_UPDATE,
WAITING_FOR_SURVEY_UPDATE,
NEEDS_PATH_UPDATE,
UP_TO_DATE
};
QDebug &operator<<(QDebug &ds, STATE s);
enum class EVENT {
INIT_DONE,
M_AREA_NOT_READY,
M_AREA_READY,
M_AREA_PATH_CHANGED,
S_AREA_PATH_CHANGED,
CORRIDOR_PATH_CHANGED,
M_AREA_TILES_CHANGED,
M_AREA_PROGRESS_CHANGED,
J_AREA_UPDATED,
DEPOT_CHANGED,
SURVEY_DESTROYED,
MISSION_ITEMS_DESTROYED,
SURVEY_UPDATE_TRIGGERED,
SURVEY_UPDATED,
PATH_CHANGED,
PATH_UPDATED
};
QDebug &operator<<(QDebug &ds, EVENT s);
class WimaStateMachine : public QObject {
Q_OBJECT
public:
explicit WimaStateMachine(QObject *parent = nullptr);
STATE state();
void updateState(EVENT e);
bool upToDate();
bool surveyReady();
signals:
void stateChanged();
void upToDateChanged();
void surveyReadyChanged();
private:
void setState(STATE s);
bool surveyReady(STATE s);
bool upToDate(STATE s);
STATE _state;
};
} // namespace wima_planer_detail
src/Wima/call_once.h deleted 100644 → 0
View file @ 458ab210
#ifndef CALL_ONCE_H
#define CALL_ONCE_H
#include <QAtomicInt>
#include <QMutex>
#include <QThread>
#include <QThreadStorage>
#include <QWaitCondition>
#include <QtGlobal>
namespace CallOnce {
enum ECallOnce { CO_Request, CO_InProgress, CO_Finished };
Q_GLOBAL_STATIC(QThreadStorage<QAtomicInt *>, once_flag)
} // namespace CallOnce
template <class Function>
inline static void qCallOnce(Function func, QBasicAtomicInt &flag) {
using namespace CallOnce;
#if QT_VERSION < 0x050000
int protectFlag = flag.fetchAndStoreAcquire(flag);
#elif QT_VERSION >= 0x050000
int protectFlag = flag.fetchAndStoreAcquire(flag.load());
#endif
if (protectFlag == CO_Finished)
return;
if (protectFlag == CO_Request &&
flag.testAndSetRelaxed(protectFlag, CO_InProgress)) {
func();
flag.fetchAndStoreRelease(CO_Finished);
} else {
do {
QThread::yieldCurrentThread();
} while (!flag.testAndSetAcquire(CO_Finished, CO_Finished));
}
}
template <class Function> inline static void qCallOncePerThread(Function func) {
using namespace CallOnce;
if (!once_flag()->hasLocalData()) {
once_flag()->setLocalData(new QAtomicInt(CO_Request));
qCallOnce(func, *once_flag()->localData());
}
}
#endif // CALL_ONCE_H
src/Wima/json/CircularSurvey.SettingsGroup.json deleted 100644 → 0
View file @ 458ab210
{
"version": 1,
"fileType": "FactMetaData",
"QGC.MetaData.Facts":
[
{
"name": "TransectDistance",
"shortDescription": "The distance between transects.",
"type": "double",
"units": "m",
"min": 0.3,
"decimalPlaces": 1,
"defaultValue": 20.0
},
{
"name": "Alpha",
"shortDescription": "Angle discretisation or transect angle (depending on type).",
"type": "double",
"units": "Deg",
"min": 0,
"max": 180,
"decimalPlaces": 1,
"defaultValue": 5.0
},
{
"name": "MinLength",
"shortDescription": "The minimal transect length.",
"type": "double",
"units": "m",
"min": 0.3,
"decimalPlaces": 1,
"defaultValue": 5.0
},
{
"name": "Type",
"shortDescription": "Survey Type.",
"type": "uint64",
"defaultValue": 0
},
{
"name": "Variant",
"shortDescription": "Route variant.",
"type": "uint64",
"defaultValue": 0
},
{
"name": "NumRuns",
"shortDescription": "The number of runs.",
"type": "uint64",
"min": 1,
"defaultValue": 1
},
{
"name": "Run",
"shortDescription": "The current run.",
"type": "uint64",
"defaultValue": 0
}
]
}
src/Wima/json/WimaController.SettingsGroup.json deleted 100644 → 0
View file @ 458ab210
[
{
"name": "EnableWimaController",
"shortDescription": "Enables or disables the WimaController, which performes different tasks inside the flight view window.",
"type": "bool",
"defaultValue": 1
},
{
"name": "OverlapWaypoints",
"shortDescription": "Determines the number of overlapping waypoints between two consecutive mission phases.",
"type": "uint32",
"defaultValue": 2
},
{
"name": "MaxWaypointsPerPhase",
"shortDescription": "Determines the maximum number of waypoints per phase. This number does not include the arrival and return path.",
"type": "uint32",
"min" : 1,
"defaultValue": 30
},
{
"name": "StartWaypointIndex",
"shortDescription": "The index of the start waypoint for the next mission phase.",
"type": "uint32",
"min" : 1,
"defaultValue": 1
},
{
"name": "ShowAllMissionItems",
"shortDescription": "Determines whether the mission items of the overall mission are displayed or not.",
"type": "bool",
"defaultValue": 1
},
{
"name": "ShowCurrentMissionItems",
"shortDescription": "Determines whether the mission items of the current mission phase are displayed or not.",
"type": "bool",
"defaultValue": 1
},
{
"name": "FlightSpeed",
"shortDescription": "The phase flight speed.",
"type": "double",
"min" : 0.3,
"max" : 5,
"defaultValue": 2
},
{
"name": "ArrivalReturnSpeed",
"shortDescription": "The flight speed for arrival and return path.",
"type": "double",
"min" : 0.3,
"max" : 5,
"defaultValue": 5
},
{
"name": "Altitude",
"shortDescription": "The mission altitude.",
"type": "double",
"min" : 1,
"defaultValue": 5
},
{
"name": "Reverse",
"shortDescription": "Reverses the phase direction. Phases go from high to low waypoint numbers, if checked.",
"type": "bool",
"defaultValue": false
},
{
"name": "SnakeTileWidth",
"shortDescription": "Snake: Tile Width.",
"type": "double",
"min" : 0.3,
"defaultValue": 5
},
{
"name": "SnakeTileHeight",
"shortDescription": "Snake: Tile Height.",
"type": "double",
"min" : 0.3,
"defaultValue": 5
},
{
"name": "SnakeMinTileArea",
"shortDescription": "Snake: Minimal Tile Area.",
"type": "double",
"min" : 0.1,
"defaultValue": 5
},
{
"name": "SnakeLineDistance",
"shortDescription": "Snake: Line Distance.",
"type": "double",
"min" : 0.3,
"defaultValue": 2
},
{
"name": "SnakeMinTransectLength",
"shortDescription": "Snake: Minimal Transect Length.",
"type": "double",
"min" : 0.3,
"defaultValue": 2
}
]
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