diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 64a35ef4f7613ddcf059457a09bc2d5f21e0bb72..ba496589697f2ec2212d1228bbba7b1e1048ece9 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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 diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 0ffb5bcccece4e31e9db2293fc18fd682484bf5b..9b4e3f387b2ab5c0ef272fc5f841f0350124bea9 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -289,7 +289,6 @@ src/WimaView/WimaServiceAreaEditor.qml src/WimaView/WimaServiceAreaMapVisual.qml src/WimaView/WimaToolBar.qml - src/WimaView/WimaView.qml src/VehicleSetup/SetupParameterEditor.qml src/VehicleSetup/SetupView.qml src/PlanView/SimpleItemEditor.qml diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 3b78dcbd6b1488b7e674cfd8bc983b5d0716c5f7..b084c5bcdc4c16881dabf83c9b5741bedca7e2ae 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -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("Wima", 1, 0, "WimaController"); - qmlRegisterType("Wima", 1, 0, "WimaPlaner"); qmlRegisterType("Wima", 1, 0, "NemoInterface"); qmlRegisterInterface("GeneratorBase"); qmlRegisterType("Wima", 1, 0, diff --git a/src/QmlControls/FlightPathSegment.h b/src/QmlControls/FlightPathSegment.h index 1eb0797ff73bea9a6bc0b44b93d61d4e0e065353..638bdd4e1abcae67cb3ce1f3632e6f627e2160d8 100644 --- a/src/QmlControls/FlightPathSegment.h +++ b/src/QmlControls/FlightPathSegment.h @@ -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; } diff --git a/src/Wima/.directory b/src/Wima/.directory deleted file mode 100644 index fb64c37e492746510948a33681efb302a4e63cac..0000000000000000000000000000000000000000 --- a/src/Wima/.directory +++ /dev/null @@ -1,4 +0,0 @@ -[Dolphin] -Timestamp=2020,7,8,11,13,28 -Version=4 -ViewMode=1 diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc deleted file mode 100644 index c5c2ffc2cf17d91e182e7d6ba935af1699590684..0000000000000000000000000000000000000000 --- a/src/Wima/CircularSurvey.cc +++ /dev/null @@ -1,646 +0,0 @@ -#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 -#include - -#include "CircularGenerator.h" -#include "LinearGenerator.h" - -// ToDo: Check what happened to _transectsDirty - -QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog") - -template -constexpr typename std::underlying_type::type integral(T value) { - return static_cast::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()), - _pWorker(std::make_unique()) { - - 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(this->_areaData); - registerGenerator(lg->name(), lg); - auto cg = std::make_shared(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 versionKeyInfoList = { - {JsonHelper::jsonVersionKey, QJsonValue::Double, true}, - }; - if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList, - errorString)) { - return false; - } - - int version = complexObject[JsonHelper::jsonVersionKey].toInt(); - if (version != 1) { - errorString = tr("Survey items do not support version %1").arg(version); - return false; - } - - QList keyInfoList = { - {VisualMissionItem::jsonTypeKey, QJsonValue::String, true}, - {ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true}, - {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::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::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 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::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 variantVector; - const auto nSolutions = pRoute->solutionVector.size(); - - for (std::size_t j = 0; j < nSolutions; ++j) { - Variant var{QList()}; - 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 -*/ diff --git a/src/Wima/CircularSurvey.h b/src/Wima/CircularSurvey.h deleted file mode 100644 index 50ab8edeef730fb11003f856951973c20d6f7728..0000000000000000000000000000000000000000 --- a/src/Wima/CircularSurvey.h +++ /dev/null @@ -1,131 +0,0 @@ -#pragma once - -#include -#include -#include - -#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; - using PtrRoutingData = std::shared_ptr; - using PtrWorker = std::unique_ptr; - using Transects = QList>; - 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 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 _metaDataMap; - SettingsFact _variant; - QStringList _variantNames; - - // Area data - std::shared_ptr _pAreaData; - - // Generators - QList _generatorList; - QStringList _generatorNameList; - PtrGenerator _pGenerator; - - // Routing. - QVector _variantVector; - PtrWorker _pWorker; -}; diff --git a/src/Wima/GenericSingelton.cpp b/src/Wima/GenericSingelton.cpp deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000 diff --git a/src/Wima/GenericSingelton.h b/src/Wima/GenericSingelton.h deleted file mode 100644 index 93e1ec0d5b684d8059b896f607a4851018b611ae..0000000000000000000000000000000000000000 --- a/src/Wima/GenericSingelton.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once -#include "call_once.h" -#include -#include - -template class GenericSingelton { -private: - typedef T *(*CreateInstanceFunction)(); - -public: - static T *instance(CreateInstanceFunction create); - -private: - static void init(); - - GenericSingelton(); - ~GenericSingelton(); - Q_DISABLE_COPY(GenericSingelton) - static QBasicAtomicPointer create; - static QBasicAtomicInt flag; - static QBasicAtomicPointer tptr; - bool inited; -}; - -template -T *GenericSingelton::instance(CreateInstanceFunction create) { - GenericSingelton::create.store(reinterpret_cast(create)); - qCallOnce(init, flag); - return (T *)tptr.load(); -} - -template void GenericSingelton::init() { - static GenericSingelton singleton; - if (singleton.inited) { - CreateInstanceFunction createFunction = - (CreateInstanceFunction)GenericSingelton::create.load(); - tptr.store(createFunction()); - } -} - -template GenericSingelton::GenericSingelton() { inited = true; } - -template GenericSingelton::~GenericSingelton() { - T *createdTptr = (T *)tptr.fetchAndStoreOrdered(nullptr); - if (createdTptr) { - delete createdTptr; - } - create.store(nullptr); -} - -template -QBasicAtomicPointer - GenericSingelton::create = Q_BASIC_ATOMIC_INITIALIZER(nullptr); -template -QBasicAtomicInt GenericSingelton::flag = - Q_BASIC_ATOMIC_INITIALIZER(CallOnce::CO_Request); -template -QBasicAtomicPointer - GenericSingelton::tptr = Q_BASIC_ATOMIC_INITIALIZER(nullptr); diff --git a/src/Wima/Geometry/GenericCircle.h b/src/Wima/Geometry/GenericCircle.h deleted file mode 100644 index 8d35c785ac0baf8615518e501e7d115bf1de70ff..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/GenericCircle.h +++ /dev/null @@ -1,154 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "boost/units/systems/angle/degrees.hpp" -#include "boost/units/systems/si.hpp" - -template auto get(const Point &p); - -namespace bu = boost::units; - -namespace qty { -using Length = bu::quantity; -using Angle = bu::quantity; -using Degree = bu::quantity; -using Radian = bu::quantity; -} // namespace qty - -template 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 -void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2, - Container &c); -template -void approximate(Circle &circ, long n, Container &c); - -// Impl. -template -GenericCircle::GenericCircle() : _r(0), _origin(0, 0) {} - -template -GenericCircle::GenericCircle(NumberType rad, Point ori) - : _r(rad), _origin(ori) {} - -/*! - * Returns a polygon with \a n corners which approximates the - * circle. - * - * \sa Point - */ - -template -void GenericCircle::setRadius(NumberType rad) { - if (rad >= 0) { - this->_r = rad; - } -} - -template -void GenericCircle::setOrigin(const Point &ori) { - this->_origin = ori; -} - -template -NumberType GenericCircle::radius() const { - return this->_r; -} - -template -const Point &GenericCircle::origin() const { - return this->_origin; -} - -template -Point &GenericCircle::origin() { - return this->_origin; -} - -template -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>; - auto x0 = get<0>(circ.origin()); - auto y0 = get<1>(circ.origin()); - auto r = circ.radius(); - using NumberType = std::remove_cv_t>; - 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 -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>; - auto x0 = get<0>(circ.origin()); - auto y0 = get<1>(circ.origin()); - auto r = circ.radius(); - using NumberType = std::remove_cv_t>; - 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; - } -} diff --git a/src/Wima/Geometry/GenericPolygon.h b/src/Wima/Geometry/GenericPolygon.h deleted file mode 100644 index 974f49da6d504627f3f60b04bfe7f057643b6cee..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/GenericPolygon.h +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include -#include - -#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h" - -template class ContainerType = QVector> -class GenericPolygon{ // - typedef ros_bridge::messages::geometry_msgs::polygon::GenericPolygon 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 &path() const {return _polygon.points();} - ContainerType &path() {return _polygon.points();} - -private: - Polygon _polygon; - -}; diff --git a/src/Wima/Geometry/GenericPolygonArray.h b/src/Wima/Geometry/GenericPolygonArray.h deleted file mode 100644 index 68dfa14ae0afa5490df03e7fb57d348919347a12..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/GenericPolygonArray.h +++ /dev/null @@ -1,58 +0,0 @@ -#pragma once - -#include "QmlObjectListModel.h" - -#include -#include - -template 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 &polygons() const { - return _polygons; - } - - ContainerType &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 _polygons; - class QmlObjectListModel _objs; - bool _dirty; - -}; - - diff --git a/src/Wima/Geometry/GeoPoint3D.cpp b/src/Wima/Geometry/GeoPoint3D.cpp deleted file mode 100644 index 2d5bd3d59366fa4e1c764f943e148dda65197651..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/GeoPoint3D.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#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; -} diff --git a/src/Wima/Geometry/GeoPoint3D.h b/src/Wima/Geometry/GeoPoint3D.h deleted file mode 100644 index 63b221d117e52c061d27e3f48bcea27e3cc7f3f4..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/GeoPoint3D.h +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include -#include - -#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); - -}; - - diff --git a/src/Wima/Geometry/PlanimetryCalculus.cc b/src/Wima/Geometry/PlanimetryCalculus.cc deleted file mode 100644 index d8053d351c3d131cdfb41c5c7ca6de26cd0a5d8e..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/PlanimetryCalculus.cc +++ /dev/null @@ -1,774 +0,0 @@ -#include "PlanimetryCalculus.h" - -template 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}, 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 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> 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 &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 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 &intersectionPoints, - NeighbourVector &neighbourList) { - QVector types; - return intersects(circle, polygon, intersectionPoints, neighbourList, types); -} - -bool intersects(const Circle &circle, const QPolygonF &polygon, - QVector &intersectionPoints, - IntersectVector &typeList) { - NeighbourVector neighbourList; - return intersects(circle, polygon, intersectionPoints, neighbourList, - typeList); -} - -bool intersects(const Circle &circle, const QPolygonF &polygon, - QVector &intersectionPoints) { - NeighbourVector neighbourList; - return intersects(circle, polygon, intersectionPoints, neighbourList); -} - -bool intersects(const Circle &circle, const QPolygonF &polygon) { - QVector 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::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::infinity(); -// } else { -// k2 = dy2/dx2; -// d2 = line2.y1()-k2*line2.x1(); -// } - -// if ( !qFuzzyCompare(k1, std::numeric_limits::infinity()) -// && !qFuzzyCompare(k1, std::numeric_limits::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. -*/ diff --git a/src/Wima/Geometry/PlanimetryCalculus.h b/src/Wima/Geometry/PlanimetryCalculus.h deleted file mode 100644 index c54946dc5cc2fad09f3d6f205c534da80caa075c..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/PlanimetryCalculus.h +++ /dev/null @@ -1,124 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "GenericCircle.h" -#include "PolygonCalculus.h" - -using Circle = GenericCircle; - -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> NeighbourVector; -typedef QVector QPointFVector; -typedef QVector 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 &intersectionPoints); -bool intersects(const Circle &circle, const QPolygonF &polygon, - QVector &intersectionPoints, - IntersectVector &typeList); -bool intersects(const Circle &circle, const QPolygonF &polygon, - QVector &intersectionPoints, - NeighbourVector &neighbourList); -bool intersects(const Circle &circle, const QPolygonF &polygon, - QVector &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 int signum(T val) - * Returns the signum of a value \a val. - * - * \sa QPair, QVector - */ -template int signum(T val) { return (T(0) < val) - (val < T(0)); } -} // namespace PlanimetryCalculus diff --git a/src/Wima/Geometry/PolygonCalculus.cc b/src/Wima/Geometry/PolygonCalculus.cc deleted file mode 100644 index a296ed1e98b74e23bef029f011e279f5e05d30ed..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/PolygonCalculus.cc +++ /dev/null @@ -1,578 +0,0 @@ -#include "PolygonCalculus.h" -#include "PlanimetryCalculus.h" -#include "Wima/OptimisationTools.h" - -#include - -#include - -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 = 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> neighbourList; - QVector 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::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 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 2) { - - // Walk the edges, offsetting by theauto distance specified distance - QList 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 &convexPolygons) - { - // Original Code SurveyComplexItem::_PolygonDecomposeConvex() - // this follows "Mark Keil's Algorithm" https://mpen.ca/406/keil - int decompSize = std::numeric_limits::max(); - if (polygon.size() < 3) return; - if (polygon.size() == 3) { - convexPolygons << polygon; - return; - } - - QList 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::max(); - continue; - } - - // recursion - QList polyLeftDecomposed{}; - decomposeToConvex(polyLeft, polyLeftDecomposed); - - QList 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::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 &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 elementList; - elementList.append(startVertex); - elementList.append(endVertex); - for (auto vertex : polygon) elementList.append(vertex); - const int listSize = elementList.size(); - - - std::function distanceDij = [bigPolygon, elementList](const int ind1, const int ind2) -> double { - QPointF p1 = elementList[ind1]; - QPointF p2 = elementList[ind2]; - double dist = std::numeric_limits::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 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 - diff --git a/src/Wima/Geometry/PolygonCalculus.h b/src/Wima/Geometry/PolygonCalculus.h deleted file mode 100644 index 6914a1555c855f5e5aebe120d89bda1f68e784a6..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/PolygonCalculus.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - - -#include -#include -#include - - - -namespace PolygonCalculus { - - enum JoinPolygonError { NotSimplePolygon, PolygonJoined, Disjoint, PathSizeLow, Error}; - - typedef QList QVector3DList; - typedef QList QPointFList; - typedef QVector 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 &convexPolygons); - bool shortestPath (const QPolygonF &polygon, const QPointF &startVertex, const QPointF &endVertex, QVector &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); -} - - diff --git a/src/Wima/Geometry/TestPolygonCalculus.cpp b/src/Wima/Geometry/TestPolygonCalculus.cpp deleted file mode 100644 index 474e43b25751730c4c7342f26a6a89b86f777792..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/TestPolygonCalculus.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "TestPolygonCalculus.h" -#include "PolygonCalculus.h" -#include -#include - -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); - } - -} diff --git a/src/Wima/Geometry/TestPolygonCalculus.h b/src/Wima/Geometry/TestPolygonCalculus.h deleted file mode 100644 index 0212676bbfff46daafa97b2275ca0ac86d287905..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/TestPolygonCalculus.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef TESTPOLYGONCALCULUS_H -#define TESTPOLYGONCALCULUS_H - - - -namespace TestPolygonCalculus { - void test(); -} -#endif // TESTPOLYGONCALCULUS_H diff --git a/src/Wima/Geometry/WimaArea.cc b/src/Wima/Geometry/WimaArea.cc deleted file mode 100644 index d4681a10500fd455c2afbd609fc1bd0dda855bc7..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaArea.cc +++ /dev/null @@ -1,548 +0,0 @@ -#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 GeoPolygon1 = area1.coordinateList(); - QList 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 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 -*/ diff --git a/src/Wima/Geometry/WimaArea.h b/src/Wima/Geometry/WimaArea.h deleted file mode 100644 index 69fbeff5e1c5c712ad990a9aab63d0e456cb4b06..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaArea.h +++ /dev/null @@ -1,114 +0,0 @@ -#pragma once - -#include "QGCGeo.h" -#include "QGCMapPolygon.h" -#include "QGCMapPolyline.h" -#include "Vehicle.h" -#include "qobject.h" -#include -#include -#include - -#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 _metaDataMap; - SettingsFact _borderPolygonOffset; - SettingsFact _showBorderPolygon; - - QGCMapPolygon _borderPolygon; - - bool _wimaAreaInteractive; -}; diff --git a/src/Wima/Geometry/WimaAreaData.cc b/src/Wima/Geometry/WimaAreaData.cc deleted file mode 100644 index ca977dc226454138182d6dd182a09cb5e4bce920..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaAreaData.cc +++ /dev/null @@ -1,138 +0,0 @@ -#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 &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 ¢er) { - 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 &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()); - } -} - -/*! - * \fn void WimaAreaData::setPath(const QList &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 &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 - */ diff --git a/src/Wima/Geometry/WimaAreaData.h b/src/Wima/Geometry/WimaAreaData.h deleted file mode 100644 index b9348aa5fcfe9ad63271cc7aea27a023665f7101..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaAreaData.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include - -#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 &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 &coordinateList); - void setPath(const QVariantList &coordinateList); - void setCenter(const QGeoCoordinate ¢er); - -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 &coordinateList); - void _setPathImpl(const QVariantList &coordinateList); - // Member Variables - mutable QVariantList _path; - QList _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); diff --git a/src/Wima/Geometry/WimaCorridor.cc b/src/Wima/Geometry/WimaCorridor.cc deleted file mode 100644 index 52c0659d2d27fc8e16fc05744a66ab46e1450c81..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaCorridor.cc +++ /dev/null @@ -1,58 +0,0 @@ -#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. -*/ diff --git a/src/Wima/Geometry/WimaCorridor.h b/src/Wima/Geometry/WimaCorridor.h deleted file mode 100644 index cef6494ed5ad566f847f742d7addc58a63d71df5..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaCorridor.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include "WimaArea.h" -#include - -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(); -}; diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc deleted file mode 100644 index eee40a95dd1984f2e32816f095cc1acf13e2ef7f..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaMeasurementArea.cc +++ /dev/null @@ -1,404 +0,0 @@ -#include "WimaMeasurementArea.h" -#include "QtConcurrentRun" -#include "SnakeTile.h" -#include "snake.h" - -#include - -#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(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 &WimaMeasurementArea::progress() const { - return this->_progress; -} - -QVector 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 &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 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::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::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(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::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>::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 - */ diff --git a/src/Wima/Geometry/WimaMeasurementArea.h b/src/Wima/Geometry/WimaMeasurementArea.h deleted file mode 100644 index 07266032584d19d5dfcdf81560e8bc77e4152fe2..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaMeasurementArea.h +++ /dev/null @@ -1,116 +0,0 @@ -#pragma once - -#include -#include -#include - -#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 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 &progress() const; - QVector 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 &p); - -private slots: - void doUpdate(); - void deferUpdate(); - void storeTiles(); - void disableUpdate(); - void enableUpdate(); - -private: - // Member Methodes - void init(); - void setState(STATE s); - - // Members - QMap _metaDataMap; - - SettingsFact _tileHeight; - SettingsFact _tileWidth; - SettingsFact _minTileAreaPercent; // 0..100 - SettingsFact _showTiles; - - QVector _progress; - // Tile stuff. - // Tile stuff. - mutable QTimer _timer; - using DataPtr = std::shared_ptr; - mutable STATE _state; - mutable TileData _tileData; - mutable QFutureWatcher _watcher; -}; diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.cc b/src/Wima/Geometry/WimaMeasurementAreaData.cc deleted file mode 100644 index 5d0b68fcc3b16aaf18b248600ab6404002e08378..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaMeasurementAreaData.cc +++ /dev/null @@ -1,137 +0,0 @@ -#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 &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 &WimaMeasurementAreaData::progress() const { - return this->_progress; -} - -QVector &WimaMeasurementAreaData::progress() { return this->_progress; } - -bool operator==(const WimaMeasurementAreaData &m1, - const WimaMeasurementArea &m2) { - return operator==(*static_cast(&m1), - *static_cast(&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; -} diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.h b/src/Wima/Geometry/WimaMeasurementAreaData.h deleted file mode 100644 index dde208291fdd5c69080d651e9f828dcdd3852bf7..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaMeasurementAreaData.h +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include -#include - -#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 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 &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 &progress() const; - QVector &progress(); - - static const char *typeString; - -signals: - void tileDataChanged(); - void progressChanged(); - -private: - TileData _tileData; - QVector _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); diff --git a/src/Wima/Geometry/WimaServiceArea.cc b/src/Wima/Geometry/WimaServiceArea.cc deleted file mode 100644 index 84b6c6afd10848675321f2039f506eb6d1b5deb3..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaServiceArea.cc +++ /dev/null @@ -1,119 +0,0 @@ -#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::infinity(); - int minIndex = 0; - for (int idx = 0; idx < this->pathModel().count(); ++idx) { - const QObject *obj = this->pathModel()[idx]; - const auto *vertex = qobject_cast(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(minIndex)->coordinate()); - } else if (this->pathModel().count() > 0) { - // Use first coordinate. - this->setDepot(this->pathModel().value(0)->coordinate()); - } - } - }); -} diff --git a/src/Wima/Geometry/WimaServiceArea.h b/src/Wima/Geometry/WimaServiceArea.h deleted file mode 100644 index 08ff94c34ccec0a73ebc2f8b09228892b5e9b6d9..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaServiceArea.h +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include "WimaArea.h" -#include "WimaTrackerPolyline.h" -#include - -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; -}; diff --git a/src/Wima/Geometry/WimaServiceAreaData.cc b/src/Wima/Geometry/WimaServiceAreaData.cc deleted file mode 100644 index ef52635dfe56dab66a887408c2f5315a99cc268f..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaServiceAreaData.cc +++ /dev/null @@ -1,66 +0,0 @@ -#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 - */ diff --git a/src/Wima/Geometry/WimaServiceAreaData.h b/src/Wima/Geometry/WimaServiceAreaData.h deleted file mode 100644 index 2b93a20964008977061f992314174feee299d408..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/WimaServiceAreaData.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include "QGeoCoordinate" -#include - -#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; -}; diff --git a/src/Wima/Geometry/json/WimaArea.SettingsGroup.json b/src/Wima/Geometry/json/WimaArea.SettingsGroup.json deleted file mode 100644 index 321fa4747e2e973f30e8d0831bbd879620119e7a..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/json/WimaArea.SettingsGroup.json +++ /dev/null @@ -1,17 +0,0 @@ -[ -{ - "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 -} -] diff --git a/src/Wima/Geometry/json/WimaMeasurementArea.SettingsGroup.json b/src/Wima/Geometry/json/WimaMeasurementArea.SettingsGroup.json deleted file mode 100644 index c4fda54e2296fc8eaf992feaac5054d0aa969c90..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/json/WimaMeasurementArea.SettingsGroup.json +++ /dev/null @@ -1,45 +0,0 @@ -[ -{ - "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 -} -] diff --git a/src/Wima/Geometry/testplanimetrycalculus.h b/src/Wima/Geometry/testplanimetrycalculus.h deleted file mode 100644 index 5977e3803431ece2de612d0c958497500ee298a2..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/testplanimetrycalculus.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include "PlanimetryCalculus.h" - -#include -#include - -namespace TestPlanimetryCalculus { - - void test(); - -} diff --git a/src/Wima/OptimisationTools.cc b/src/Wima/OptimisationTools.cc deleted file mode 100644 index 0d03739df441d601eb955d9c7ff7bd85e3f762c0..0000000000000000000000000000000000000000 --- a/src/Wima/OptimisationTools.cc +++ /dev/null @@ -1,105 +0,0 @@ -#include "OptimisationTools.h" - -namespace OptimisationTools { - namespace { - - } // end anonymous namespace - - bool dijkstraAlgorithm(const int numElements, int startIndex, int endIndex, QVector &elementPath, std::function 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::infinity(); - }; - - // The list with all Nodes (elements) - QVector 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 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::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 diff --git a/src/Wima/OptimisationTools.h b/src/Wima/OptimisationTools.h deleted file mode 100644 index 11c37431dc27d7ae63663b741889d775d5dd14d1..0000000000000000000000000000000000000000 --- a/src/Wima/OptimisationTools.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once -#include -#include - -#include -#include - -namespace OptimisationTools { - bool dijkstraAlgorithm(const int numElements, int startIndex, int endIndex, QVector &elementPath, std::function distanceDij); -} - - diff --git a/src/Wima/RoutingThread.cpp b/src/Wima/RoutingThread.cpp deleted file mode 100644 index f45bb3c0349231b9e91a4203ef6c38a7ad95a847..0000000000000000000000000000000000000000 --- a/src/Wima/RoutingThread.cpp +++ /dev/null @@ -1,129 +0,0 @@ -#include "RoutingThread.h" -// std -#include -// Qt -#include - -#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"); }); -} - -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::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."; -} diff --git a/src/Wima/RoutingThread.h b/src/Wima/RoutingThread.h deleted file mode 100644 index bc5a3f11389c15e847db2438e26abe9c316adc93..0000000000000000000000000000000000000000 --- a/src/Wima/RoutingThread.h +++ /dev/null @@ -1,63 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "snake.h" -#include -#include -#include -#include - -struct RoutingData { - snake::Transects transects; - std::vector 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; - -public: - using PtrRoutingData = shared_ptr; - using Generator = std::function; - using Consumer = std::function; - - 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; -}; diff --git a/src/Wima/Snake/CircularGenerator.cpp b/src/Wima/Snake/CircularGenerator.cpp deleted file mode 100644 index 838dae395b68dc2cc7019203fb458542ec4331fd..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/CircularGenerator.cpp +++ /dev/null @@ -1,465 +0,0 @@ -#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 &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::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>(); - 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(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 &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 distances; - distances.reserve(polygon.outer().size()); - std::vector 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 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; - for (auto §or : 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 processedTiles; - for (const auto &tile : tiles) { - ClipperLib::Path path; - for (const auto &v : tile.outer()) { - path.push_back(ClipperLib::IntPoint{ - static_cast(v.get<0>() * CLIPPER_SCALE), - static_cast(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(vertex.X) / CLIPPER_SCALE; - auto y = static_cast(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::high_resolution_clock::now() - s1) - .count() - << " ms"; - return true; - } - } - return false; -} - -} // namespace routing diff --git a/src/Wima/Snake/CircularGenerator.h b/src/Wima/Snake/CircularGenerator.h deleted file mode 100644 index d4458a2292ea9b90317d2be338f0848b6fe56503..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/CircularGenerator.h +++ /dev/null @@ -1,63 +0,0 @@ -#include "GeneratorBase.h" - -#include - -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 _metaDataMap; - SettingsFact _distance; - SettingsFact _deltaAlpha; - SettingsFact _minLength; -}; - -} // namespace routing diff --git a/src/Wima/Snake/GeneratorBase.cc b/src/Wima/Snake/GeneratorBase.cc deleted file mode 100644 index bc315e54b9bd9319d433208f20a75bf29a53c5bd..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/GeneratorBase.cc +++ /dev/null @@ -1,31 +0,0 @@ -#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 diff --git a/src/Wima/Snake/GeneratorBase.h b/src/Wima/Snake/GeneratorBase.h deleted file mode 100644 index 96605d0543d90c260e02a7127ce86890dbd13c1d..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/GeneratorBase.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include - -#include -#include - -#include "snake.h" - -#include "Wima/WimaPlanData.h" - -namespace routing { - -class GeneratorBase : public QObject { - Q_OBJECT -public: - using Data = std::shared_ptr; - using Generator = std::function; - - 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 diff --git a/src/Wima/Snake/LinearGenerator.cpp b/src/Wima/Snake/LinearGenerator.cpp deleted file mode 100644 index 7f5c4b739f038850e1a6165814f4cb157cc7aef0..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/LinearGenerator.cpp +++ /dev/null @@ -1,336 +0,0 @@ -#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 &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::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>(); - 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(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 &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 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 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 rotate_back( - -angle.value() * 180 / M_PI); - bg::transform(transect, temp_transect, rotate_back); - // to clipper - ClipperLib::IntPoint c1{static_cast( - temp_transect[0].get<0>() * CLIPPER_SCALE), - static_cast( - temp_transect[0].get<1>() * CLIPPER_SCALE)}; - ClipperLib::IntPoint c2{static_cast( - temp_transect[1].get<0>() * CLIPPER_SCALE), - static_cast( - 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(vertex.get<0>() * CLIPPER_SCALE), - static_cast(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 processedTiles; - for (const auto &tile : tiles) { - ClipperLib::Path path; - for (const auto &v : tile.outer()) { - path.push_back(ClipperLib::IntPoint{ - static_cast(v.get<0>() * CLIPPER_SCALE), - static_cast(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(clipperTransect[0].X) / CLIPPER_SCALE, - static_cast(clipperTransect[0].Y) / CLIPPER_SCALE}; - snake::FPoint v2{ - static_cast(clipperTransect[1].X) / CLIPPER_SCALE, - static_cast(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::high_resolution_clock::now() - s1) - .count() - << " ms"; - return true; - } - } - return false; -} -} // namespace routing diff --git a/src/Wima/Snake/LinearGenerator.h b/src/Wima/Snake/LinearGenerator.h deleted file mode 100644 index ba63cf51a322f7c6e3d0b3cdc30ae3a6b5c6183e..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/LinearGenerator.h +++ /dev/null @@ -1,46 +0,0 @@ -#include "GeneratorBase.h" - -#include - -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 _metaDataMap; - SettingsFact _distance; - SettingsFact _alpha; - SettingsFact _minLength; -}; - -} // namespace routing diff --git a/src/Wima/Snake/NemoInterface.cpp b/src/Wima/Snake/NemoInterface.cpp deleted file mode 100644 index dd07f8d3999ed8127796f5562e89f869698951d3..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/NemoInterface.cpp +++ /dev/null @@ -1,468 +0,0 @@ -#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 - -#include - -#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; -using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr; -using UniqueLock = std::unique_lock; -using SharedLock = std::shared_lock; -using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr; - -class NemoInterface::Impl { - using TimePoint = std::chrono::time_point; - -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 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 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; -static StatusMap statusMap{ - std::make_pair( - NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"), - std::make_pair( - NemoInterface::STATUS::HEARTBEAT_DETECTED, "Heartbeat Detected"), - std::make_pair( - NemoInterface::STATUS::TIMEOUT, "Timeout"), - std::make_pair( - NemoInterface::STATUS::INVALID_HEARTBEAT, "Error"), - std::make_pair( - 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(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(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 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::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::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::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::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::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(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 NemoInterface::progress() const { return this->pImpl->progress(); } - -QString NemoInterface::editorQml() { - return QStringLiteral("NemoInterface.qml"); -} - -bool NemoInterface::running() { return this->pImpl->running(); } diff --git a/src/Wima/Snake/NemoInterface.h b/src/Wima/Snake/NemoInterface.h deleted file mode 100644 index 459f02917dae3f177ef96e5579db979054c80c31..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/NemoInterface.h +++ /dev/null @@ -1,57 +0,0 @@ -#pragma once - -#include -#include - -#include - -class TileData; - -class NemoInterface : public QObject { - Q_OBJECT - class Impl; - using PImpl = std::unique_ptr; - -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 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 progress() const; - QString editorQml(); - bool running(); - -signals: - void statusChanged(); - void progressChanged(); - void runningChanged(); - -private: - PImpl pImpl; -}; diff --git a/src/Wima/Snake/QNemoHeartbeat.h b/src/Wima/Snake/QNemoHeartbeat.h deleted file mode 100644 index 8e95986eb6324c4657d91ecc5965ccbf7e4cd24d..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/QNemoHeartbeat.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h" - -using QNemoHeartbeat = ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat; diff --git a/src/Wima/Snake/QNemoProgress.cc b/src/Wima/Snake/QNemoProgress.cc deleted file mode 100644 index d7dc1a396652cc9e5e39dac379334ecf7838f641..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/QNemoProgress.cc +++ /dev/null @@ -1,2 +0,0 @@ -#include "QNemoProgress.h" - diff --git a/src/Wima/Snake/QNemoProgress.h b/src/Wima/Snake/QNemoProgress.h deleted file mode 100644 index e57ec2fdf108993dff80f7d5544b6d10a77c39c2..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/QNemoProgress.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include - -#include "ros_bridge/include/messages/nemo_msgs/progress.h" - -namespace nemo = ros_bridge::messages::nemo_msgs; -typedef nemo::progress::GenericProgress QNemoProgress; diff --git a/src/Wima/Snake/SnakeThread.cc b/src/Wima/Snake/SnakeThread.cc deleted file mode 100644 index 1242e4e82cbcd3ad854b9d0cbb4688844384b1de..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeThread.cc +++ /dev/null @@ -1,540 +0,0 @@ -#include "SnakeThread.h" - -#include -#include - -#include "QGCApplication.h" -#include "QGCToolbox.h" -#include "SettingsFact.h" -#include "SettingsManager.h" -#include "WimaSettings.h" - -#include -#include -#include -#include - -#include "Wima/Snake/SnakeTiles.h" -#include "Wima/Snake/SnakeTilesLocal.h" -#include "snake.h" - -template class CommandRAII { -public: - CommandRAII(Callable &fun) : _fun(fun) {} - - ~CommandRAII() { _fun(); } - -private: - Callable _fun; -}; - -using QVariantList = QList; - -using UniqueLock = std::unique_lock; -using SharedLock = std::shared_lock; - -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 mArea; - QGeoCoordinate ENUOrigin; - QList sArea; - QList 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 tileCenterPointsENU; - QGeoCoordinate ENUOrigin; - - QVector waypoints; - QVector arrivalPath; - QVector returnPath; - QVector waypointsENU; - QVector arrivalPathENU; - QVector 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 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(this)) {} - -SnakeThread::~SnakeThread() {} - -void SnakeThread::setMeasurementArea( - const QList &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 &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 &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 &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 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 SnakeThread::waypoints() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.waypoints; -} - -QVector SnakeThread::arrivalPath() const { - SharedLock lk(this->pImpl->output.mutex); - return this->pImpl->output.arrivalPath; -} - -QVector 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::high_resolution_clock::now() - startTime) - .count() - << " ms"; -#endif - this->pImpl->output.calcInProgress.store(false); - emit calcInProgressChanged(this->pImpl->output.calcInProgress.load()); - }; - CommandRAII commandRAII(onExit); - - bool tilesValid = true; - bool progressValid = false; - snake::Length lineDistance; - snake::Length minTransectLength; - std::vector 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 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{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; - } -} diff --git a/src/Wima/Snake/SnakeThread.h b/src/Wima/Snake/SnakeThread.h deleted file mode 100644 index aa3ed1702973094db99f857f24726cdfe69d66ee..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeThread.h +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "QNemoProgress.h" -#include "SnakeTiles.h" -#include "SnakeTilesLocal.h" - -#include - -using namespace boost::units; -using Length = quantity; -using Area = quantity; - -class SnakeThread : public QThread { - Q_OBJECT - - class Impl; - using PImpl = std::unique_ptr; - -public: - SnakeThread(QObject *parent = nullptr); - ~SnakeThread() override; - - void setMeasurementArea(const QList &measurementArea); - void setServiceArea(const QList &serviceArea); - void setCorridor(const QList &corridor); - void setProgress(const QVector &progress); - - SnakeTiles tiles() const; - SnakeTilesLocal tilesENU() const; - QGeoCoordinate ENUOrigin() const; - QVariantList tileCenterPoints() const; - QVector progress() const; - bool calcInProgress() const; - QString errorMessage() const; - bool success() const; - - bool tilesUpdated(); - bool waypointsUpdated(); - bool progressUpdated(); - - QVector waypoints() const; - QVector arrivalPath() const; - QVector 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; -}; diff --git a/src/Wima/Snake/SnakeTile.cpp b/src/Wima/Snake/SnakeTile.cpp deleted file mode 100644 index 397a2e917bcbd6281144aabb62ae42f4964c20f8..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeTile.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#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; -} diff --git a/src/Wima/Snake/SnakeTile.h b/src/Wima/Snake/SnakeTile.h deleted file mode 100644 index 8117048fa64bd05fa3b5e2d4b81a89e7cc642319..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeTile.h +++ /dev/null @@ -1,21 +0,0 @@ -#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); -}; diff --git a/src/Wima/Snake/SnakeTileLocal.h b/src/Wima/Snake/SnakeTileLocal.h deleted file mode 100644 index fa323fa1434ec3fcc5746c8c93a179dfaed08f39..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeTileLocal.h +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once -#include "Wima/Geometry/GenericPolygon.h" - -using SnakeTileLocal = GenericPolygon; diff --git a/src/Wima/Snake/SnakeTiles.h b/src/Wima/Snake/SnakeTiles.h deleted file mode 100644 index e73c46022233d25bd98909c9a85a264c774c5d34..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeTiles.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once -#include "SnakeTile.h" -#include "Wima/Geometry/GenericPolygonArray.h" - -using SnakeTiles = GenericPolygonArray; diff --git a/src/Wima/Snake/SnakeTilesLocal.h b/src/Wima/Snake/SnakeTilesLocal.h deleted file mode 100644 index f7ccdc7d2365f9c5b1cbcf50ea4c9f0b259eb59f..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeTilesLocal.h +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once - -#include "Wima/Geometry/GenericPolygonArray.h" -#include "Wima/Snake/SnakeTileLocal.h" -#include -typedef GenericPolygonArray SnakeTilesLocal; diff --git a/src/Wima/Snake/WimaAreaNoVisual.qml b/src/Wima/Snake/WimaAreaNoVisual.qml deleted file mode 100644 index 12b271a23491806508986f79e1261041a52a974a..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/WimaAreaNoVisual.qml +++ /dev/null @@ -1,12 +0,0 @@ -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.") - } - -} diff --git a/src/Wima/Snake/clipper/clipper.cpp b/src/Wima/Snake/clipper/clipper.cpp deleted file mode 100644 index 65affb876d8a57d9a2d775d10d62559435cc3dce..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/clipper/clipper.cpp +++ /dev/null @@ -1,4629 +0,0 @@ -/******************************************************************************* -* * -* 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 * -* * -*******************************************************************************/ - -/******************************************************************************* -* * -* This is a translation of the Delphi Clipper library and the naming style * -* used has retained a Delphi flavour. * -* * -*******************************************************************************/ - -#include "clipper.hpp" -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ClipperLib { - -static double const pi = 3.141592653589793238; -static double const two_pi = pi *2; -static double const def_arc_tolerance = 0.25; - -enum Direction { dRightToLeft, dLeftToRight }; - -static int const Unassigned = -1; //edge not currently 'owning' a solution -static int const Skip = -2; //edge that would otherwise close a path - -#define HORIZONTAL (-1.0E+40) -#define TOLERANCE (1.0e-20) -#define NEAR_ZERO(val) (((val) > -TOLERANCE) && ((val) < TOLERANCE)) - -struct TEdge { - IntPoint Bot; - IntPoint Curr; //current (updated for every new scanbeam) - IntPoint Top; - double Dx; - PolyType PolyTyp; - EdgeSide Side; //side only refers to current side of solution poly - int WindDelta; //1 or -1 depending on winding direction - int WindCnt; - int WindCnt2; //winding count of the opposite polytype - int OutIdx; - TEdge *Next; - TEdge *Prev; - TEdge *NextInLML; - TEdge *NextInAEL; - TEdge *PrevInAEL; - TEdge *NextInSEL; - TEdge *PrevInSEL; -}; - -struct IntersectNode { - TEdge *Edge1; - TEdge *Edge2; - IntPoint Pt; -}; - -struct LocalMinimum { - cInt Y; - TEdge *LeftBound; - TEdge *RightBound; -}; - -struct OutPt; - -//OutRec: contains a path in the clipping solution. Edges in the AEL will -//carry a pointer to an OutRec when they are part of the clipping solution. -struct OutRec { - int Idx; - bool IsHole; - bool IsOpen; - OutRec *FirstLeft; //see comments in clipper.pas - PolyNode *PolyNd; - OutPt *Pts; - OutPt *BottomPt; -}; - -struct OutPt { - int Idx; - IntPoint Pt; - OutPt *Next; - OutPt *Prev; -}; - -struct Join { - OutPt *OutPt1; - OutPt *OutPt2; - IntPoint OffPt; -}; - -struct LocMinSorter -{ - inline bool operator()(const LocalMinimum& locMin1, const LocalMinimum& locMin2) - { - return locMin2.Y < locMin1.Y; - } -}; - -//------------------------------------------------------------------------------ -//------------------------------------------------------------------------------ - -inline cInt Round(double val) -{ - if ((val < 0)) return static_cast(val - 0.5); - else return static_cast(val + 0.5); -} -//------------------------------------------------------------------------------ - -inline cInt Abs(cInt val) -{ - return val < 0 ? -val : val; -} - -//------------------------------------------------------------------------------ -// PolyTree methods ... -//------------------------------------------------------------------------------ - -void PolyTree::Clear() -{ - for (PolyNodes::size_type i = 0; i < AllNodes.size(); ++i) - delete AllNodes[i]; - AllNodes.resize(0); - Childs.resize(0); -} -//------------------------------------------------------------------------------ - -PolyNode* PolyTree::GetFirst() const -{ - if (!Childs.empty()) - return Childs[0]; - else - return 0; -} -//------------------------------------------------------------------------------ - -int PolyTree::Total() const -{ - int result = (int)AllNodes.size(); - //with negative offsets, ignore the hidden outer polygon ... - if (result > 0 && Childs[0] != AllNodes[0]) result--; - return result; -} - -//------------------------------------------------------------------------------ -// PolyNode methods ... -//------------------------------------------------------------------------------ - -PolyNode::PolyNode(): Parent(0), Index(0), m_IsOpen(false) -{ -} -//------------------------------------------------------------------------------ - -int PolyNode::ChildCount() const -{ - return (int)Childs.size(); -} -//------------------------------------------------------------------------------ - -void PolyNode::AddChild(PolyNode& child) -{ - unsigned cnt = (unsigned)Childs.size(); - Childs.push_back(&child); - child.Parent = this; - child.Index = cnt; -} -//------------------------------------------------------------------------------ - -PolyNode* PolyNode::GetNext() const -{ - if (!Childs.empty()) - return Childs[0]; - else - return GetNextSiblingUp(); -} -//------------------------------------------------------------------------------ - -PolyNode* PolyNode::GetNextSiblingUp() const -{ - if (!Parent) //protects against PolyTree.GetNextSiblingUp() - return 0; - else if (Index == Parent->Childs.size() - 1) - return Parent->GetNextSiblingUp(); - else - return Parent->Childs[Index + 1]; -} -//------------------------------------------------------------------------------ - -bool PolyNode::IsHole() const -{ - bool result = true; - PolyNode* node = Parent; - while (node) - { - result = !result; - node = node->Parent; - } - return result; -} -//------------------------------------------------------------------------------ - -bool PolyNode::IsOpen() const -{ - return m_IsOpen; -} -//------------------------------------------------------------------------------ - -#ifndef use_int32 - -//------------------------------------------------------------------------------ -// Int128 class (enables safe math on signed 64bit integers) -// eg Int128 val1((long64)9223372036854775807); //ie 2^63 -1 -// Int128 val2((long64)9223372036854775807); -// Int128 val3 = val1 * val2; -// val3.AsString => "85070591730234615847396907784232501249" (8.5e+37) -//------------------------------------------------------------------------------ - -class Int128 -{ - public: - ulong64 lo; - long64 hi; - - Int128(long64 _lo = 0) - { - lo = (ulong64)_lo; - if (_lo < 0) hi = -1; else hi = 0; - } - - - Int128(const Int128 &val): lo(val.lo), hi(val.hi){} - - Int128(const long64& _hi, const ulong64& _lo): lo(_lo), hi(_hi){} - - Int128& operator = (const long64 &val) - { - lo = (ulong64)val; - if (val < 0) hi = -1; else hi = 0; - return *this; - } - - bool operator == (const Int128 &val) const - {return (hi == val.hi && lo == val.lo);} - - bool operator != (const Int128 &val) const - { return !(*this == val);} - - bool operator > (const Int128 &val) const - { - if (hi != val.hi) - return hi > val.hi; - else - return lo > val.lo; - } - - bool operator < (const Int128 &val) const - { - if (hi != val.hi) - return hi < val.hi; - else - return lo < val.lo; - } - - bool operator >= (const Int128 &val) const - { return !(*this < val);} - - bool operator <= (const Int128 &val) const - { return !(*this > val);} - - Int128& operator += (const Int128 &rhs) - { - hi += rhs.hi; - lo += rhs.lo; - if (lo < rhs.lo) hi++; - return *this; - } - - Int128 operator + (const Int128 &rhs) const - { - Int128 result(*this); - result+= rhs; - return result; - } - - Int128& operator -= (const Int128 &rhs) - { - *this += -rhs; - return *this; - } - - Int128 operator - (const Int128 &rhs) const - { - Int128 result(*this); - result -= rhs; - return result; - } - - Int128 operator-() const //unary negation - { - if (lo == 0) - return Int128(-hi, 0); - else - return Int128(~hi, ~lo + 1); - } - - operator double() const - { - const double shift64 = 18446744073709551616.0; //2^64 - if (hi < 0) - { - if (lo == 0) return (double)hi * shift64; - else return -(double)(~lo + ~hi * shift64); - } - else - return (double)(lo + hi * shift64); - } - -}; -//------------------------------------------------------------------------------ - -Int128 Int128Mul (long64 lhs, long64 rhs) -{ - bool negate = (lhs < 0) != (rhs < 0); - - if (lhs < 0) lhs = -lhs; - ulong64 int1Hi = ulong64(lhs) >> 32; - ulong64 int1Lo = ulong64(lhs & 0xFFFFFFFF); - - if (rhs < 0) rhs = -rhs; - ulong64 int2Hi = ulong64(rhs) >> 32; - ulong64 int2Lo = ulong64(rhs & 0xFFFFFFFF); - - //nb: see comments in clipper.pas - ulong64 a = int1Hi * int2Hi; - ulong64 b = int1Lo * int2Lo; - ulong64 c = int1Hi * int2Lo + int1Lo * int2Hi; - - Int128 tmp; - tmp.hi = long64(a + (c >> 32)); - tmp.lo = long64(c << 32); - tmp.lo += long64(b); - if (tmp.lo < b) tmp.hi++; - if (negate) tmp = -tmp; - return tmp; -}; -#endif - -//------------------------------------------------------------------------------ -// Miscellaneous global functions -//------------------------------------------------------------------------------ - -bool Orientation(const Path &poly) -{ - return Area(poly) >= 0; -} -//------------------------------------------------------------------------------ - -double Area(const Path &poly) -{ - int size = (int)poly.size(); - if (size < 3) return 0; - - double a = 0; - for (int i = 0, j = size -1; i < size; ++i) - { - a += ((double)poly[j].X + poly[i].X) * ((double)poly[j].Y - poly[i].Y); - j = i; - } - return -a * 0.5; -} -//------------------------------------------------------------------------------ - -double Area(const OutPt *op) -{ - const OutPt *startOp = op; - if (!op) return 0; - double a = 0; - do { - a += (double)(op->Prev->Pt.X + op->Pt.X) * (double)(op->Prev->Pt.Y - op->Pt.Y); - op = op->Next; - } while (op != startOp); - return a * 0.5; -} -//------------------------------------------------------------------------------ - -double Area(const OutRec &outRec) -{ - return Area(outRec.Pts); -} -//------------------------------------------------------------------------------ - -bool PointIsVertex(const IntPoint &Pt, OutPt *pp) -{ - OutPt *pp2 = pp; - do - { - if (pp2->Pt == Pt) return true; - pp2 = pp2->Next; - } - while (pp2 != pp); - return false; -} -//------------------------------------------------------------------------------ - -//See "The Point in Polygon Problem for Arbitrary Polygons" by Hormann & Agathos -//http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.88.5498&rep=rep1&type=pdf -int PointInPolygon(const IntPoint &pt, const Path &path) -{ - //returns 0 if false, +1 if true, -1 if pt ON polygon boundary - int result = 0; - size_t cnt = path.size(); - if (cnt < 3) return 0; - IntPoint ip = path[0]; - for(size_t i = 1; i <= cnt; ++i) - { - IntPoint ipNext = (i == cnt ? path[0] : path[i]); - if (ipNext.Y == pt.Y) - { - if ((ipNext.X == pt.X) || (ip.Y == pt.Y && - ((ipNext.X > pt.X) == (ip.X < pt.X)))) return -1; - } - if ((ip.Y < pt.Y) != (ipNext.Y < pt.Y)) - { - if (ip.X >= pt.X) - { - if (ipNext.X > pt.X) result = 1 - result; - else - { - double d = (double)(ip.X - pt.X) * (ipNext.Y - pt.Y) - - (double)(ipNext.X - pt.X) * (ip.Y - pt.Y); - if (!d) return -1; - if ((d > 0) == (ipNext.Y > ip.Y)) result = 1 - result; - } - } else - { - if (ipNext.X > pt.X) - { - double d = (double)(ip.X - pt.X) * (ipNext.Y - pt.Y) - - (double)(ipNext.X - pt.X) * (ip.Y - pt.Y); - if (!d) return -1; - if ((d > 0) == (ipNext.Y > ip.Y)) result = 1 - result; - } - } - } - ip = ipNext; - } - return result; -} -//------------------------------------------------------------------------------ - -int PointInPolygon (const IntPoint &pt, OutPt *op) -{ - //returns 0 if false, +1 if true, -1 if pt ON polygon boundary - int result = 0; - OutPt* startOp = op; - for(;;) - { - if (op->Next->Pt.Y == pt.Y) - { - if ((op->Next->Pt.X == pt.X) || (op->Pt.Y == pt.Y && - ((op->Next->Pt.X > pt.X) == (op->Pt.X < pt.X)))) return -1; - } - if ((op->Pt.Y < pt.Y) != (op->Next->Pt.Y < pt.Y)) - { - if (op->Pt.X >= pt.X) - { - if (op->Next->Pt.X > pt.X) result = 1 - result; - else - { - double d = (double)(op->Pt.X - pt.X) * (op->Next->Pt.Y - pt.Y) - - (double)(op->Next->Pt.X - pt.X) * (op->Pt.Y - pt.Y); - if (!d) return -1; - if ((d > 0) == (op->Next->Pt.Y > op->Pt.Y)) result = 1 - result; - } - } else - { - if (op->Next->Pt.X > pt.X) - { - double d = (double)(op->Pt.X - pt.X) * (op->Next->Pt.Y - pt.Y) - - (double)(op->Next->Pt.X - pt.X) * (op->Pt.Y - pt.Y); - if (!d) return -1; - if ((d > 0) == (op->Next->Pt.Y > op->Pt.Y)) result = 1 - result; - } - } - } - op = op->Next; - if (startOp == op) break; - } - return result; -} -//------------------------------------------------------------------------------ - -bool Poly2ContainsPoly1(OutPt *OutPt1, OutPt *OutPt2) -{ - OutPt* op = OutPt1; - do - { - //nb: PointInPolygon returns 0 if false, +1 if true, -1 if pt on polygon - int res = PointInPolygon(op->Pt, OutPt2); - if (res >= 0) return res > 0; - op = op->Next; - } - while (op != OutPt1); - return true; -} -//---------------------------------------------------------------------- - -bool SlopesEqual(const TEdge &e1, const TEdge &e2, bool UseFullInt64Range) -{ -#ifndef use_int32 - if (UseFullInt64Range) - return Int128Mul(e1.Top.Y - e1.Bot.Y, e2.Top.X - e2.Bot.X) == - Int128Mul(e1.Top.X - e1.Bot.X, e2.Top.Y - e2.Bot.Y); - else -#endif - return (e1.Top.Y - e1.Bot.Y) * (e2.Top.X - e2.Bot.X) == - (e1.Top.X - e1.Bot.X) * (e2.Top.Y - e2.Bot.Y); -} -//------------------------------------------------------------------------------ - -bool SlopesEqual(const IntPoint pt1, const IntPoint pt2, - const IntPoint pt3, bool UseFullInt64Range) -{ -#ifndef use_int32 - if (UseFullInt64Range) - return Int128Mul(pt1.Y-pt2.Y, pt2.X-pt3.X) == Int128Mul(pt1.X-pt2.X, pt2.Y-pt3.Y); - else -#endif - return (pt1.Y-pt2.Y)*(pt2.X-pt3.X) == (pt1.X-pt2.X)*(pt2.Y-pt3.Y); -} -//------------------------------------------------------------------------------ - -bool SlopesEqual(const IntPoint pt1, const IntPoint pt2, - const IntPoint pt3, const IntPoint pt4, bool UseFullInt64Range) -{ -#ifndef use_int32 - if (UseFullInt64Range) - return Int128Mul(pt1.Y-pt2.Y, pt3.X-pt4.X) == Int128Mul(pt1.X-pt2.X, pt3.Y-pt4.Y); - else -#endif - return (pt1.Y-pt2.Y)*(pt3.X-pt4.X) == (pt1.X-pt2.X)*(pt3.Y-pt4.Y); -} -//------------------------------------------------------------------------------ - -inline bool IsHorizontal(TEdge &e) -{ - return e.Dx == HORIZONTAL; -} -//------------------------------------------------------------------------------ - -inline double GetDx(const IntPoint pt1, const IntPoint pt2) -{ - return (pt1.Y == pt2.Y) ? - HORIZONTAL : (double)(pt2.X - pt1.X) / (pt2.Y - pt1.Y); -} -//--------------------------------------------------------------------------- - -inline void SetDx(TEdge &e) -{ - cInt dy = (e.Top.Y - e.Bot.Y); - if (dy == 0) e.Dx = HORIZONTAL; - else e.Dx = (double)(e.Top.X - e.Bot.X) / dy; -} -//--------------------------------------------------------------------------- - -inline void SwapSides(TEdge &Edge1, TEdge &Edge2) -{ - EdgeSide Side = Edge1.Side; - Edge1.Side = Edge2.Side; - Edge2.Side = Side; -} -//------------------------------------------------------------------------------ - -inline void SwapPolyIndexes(TEdge &Edge1, TEdge &Edge2) -{ - int OutIdx = Edge1.OutIdx; - Edge1.OutIdx = Edge2.OutIdx; - Edge2.OutIdx = OutIdx; -} -//------------------------------------------------------------------------------ - -inline cInt TopX(TEdge &edge, const cInt currentY) -{ - return ( currentY == edge.Top.Y ) ? - edge.Top.X : edge.Bot.X + Round(edge.Dx *(currentY - edge.Bot.Y)); -} -//------------------------------------------------------------------------------ - -void IntersectPoint(TEdge &Edge1, TEdge &Edge2, IntPoint &ip) -{ -#ifdef use_xyz - ip.Z = 0; -#endif - - double b1, b2; - if (Edge1.Dx == Edge2.Dx) - { - ip.Y = Edge1.Curr.Y; - ip.X = TopX(Edge1, ip.Y); - return; - } - else if (Edge1.Dx == 0) - { - ip.X = Edge1.Bot.X; - if (IsHorizontal(Edge2)) - ip.Y = Edge2.Bot.Y; - else - { - b2 = Edge2.Bot.Y - (Edge2.Bot.X / Edge2.Dx); - ip.Y = Round(ip.X / Edge2.Dx + b2); - } - } - else if (Edge2.Dx == 0) - { - ip.X = Edge2.Bot.X; - if (IsHorizontal(Edge1)) - ip.Y = Edge1.Bot.Y; - else - { - b1 = Edge1.Bot.Y - (Edge1.Bot.X / Edge1.Dx); - ip.Y = Round(ip.X / Edge1.Dx + b1); - } - } - else - { - b1 = Edge1.Bot.X - Edge1.Bot.Y * Edge1.Dx; - b2 = Edge2.Bot.X - Edge2.Bot.Y * Edge2.Dx; - double q = (b2-b1) / (Edge1.Dx - Edge2.Dx); - ip.Y = Round(q); - if (std::fabs(Edge1.Dx) < std::fabs(Edge2.Dx)) - ip.X = Round(Edge1.Dx * q + b1); - else - ip.X = Round(Edge2.Dx * q + b2); - } - - if (ip.Y < Edge1.Top.Y || ip.Y < Edge2.Top.Y) - { - if (Edge1.Top.Y > Edge2.Top.Y) - ip.Y = Edge1.Top.Y; - else - ip.Y = Edge2.Top.Y; - if (std::fabs(Edge1.Dx) < std::fabs(Edge2.Dx)) - ip.X = TopX(Edge1, ip.Y); - else - ip.X = TopX(Edge2, ip.Y); - } - //finally, don't allow 'ip' to be BELOW curr.Y (ie bottom of scanbeam) ... - if (ip.Y > Edge1.Curr.Y) - { - ip.Y = Edge1.Curr.Y; - //use the more vertical edge to derive X ... - if (std::fabs(Edge1.Dx) > std::fabs(Edge2.Dx)) - ip.X = TopX(Edge2, ip.Y); else - ip.X = TopX(Edge1, ip.Y); - } -} -//------------------------------------------------------------------------------ - -void ReversePolyPtLinks(OutPt *pp) -{ - if (!pp) return; - OutPt *pp1, *pp2; - pp1 = pp; - do { - pp2 = pp1->Next; - pp1->Next = pp1->Prev; - pp1->Prev = pp2; - pp1 = pp2; - } while( pp1 != pp ); -} -//------------------------------------------------------------------------------ - -void DisposeOutPts(OutPt*& pp) -{ - if (pp == 0) return; - pp->Prev->Next = 0; - while( pp ) - { - OutPt *tmpPp = pp; - pp = pp->Next; - delete tmpPp; - } -} -//------------------------------------------------------------------------------ - -inline void InitEdge(TEdge* e, TEdge* eNext, TEdge* ePrev, const IntPoint& Pt) -{ - std::memset(e, 0, sizeof(TEdge)); - e->Next = eNext; - e->Prev = ePrev; - e->Curr = Pt; - e->OutIdx = Unassigned; -} -//------------------------------------------------------------------------------ - -void InitEdge2(TEdge& e, PolyType Pt) -{ - if (e.Curr.Y >= e.Next->Curr.Y) - { - e.Bot = e.Curr; - e.Top = e.Next->Curr; - } else - { - e.Top = e.Curr; - e.Bot = e.Next->Curr; - } - SetDx(e); - e.PolyTyp = Pt; -} -//------------------------------------------------------------------------------ - -TEdge* RemoveEdge(TEdge* e) -{ - //removes e from double_linked_list (but without removing from memory) - e->Prev->Next = e->Next; - e->Next->Prev = e->Prev; - TEdge* result = e->Next; - e->Prev = 0; //flag as removed (see ClipperBase.Clear) - return result; -} -//------------------------------------------------------------------------------ - -inline void ReverseHorizontal(TEdge &e) -{ - //swap horizontal edges' Top and Bottom x's so they follow the natural - //progression of the bounds - ie so their xbots will align with the - //adjoining lower edge. [Helpful in the ProcessHorizontal() method.] - std::swap(e.Top.X, e.Bot.X); -#ifdef use_xyz - std::swap(e.Top.Z, e.Bot.Z); -#endif -} -//------------------------------------------------------------------------------ - -void SwapPoints(IntPoint &pt1, IntPoint &pt2) -{ - IntPoint tmp = pt1; - pt1 = pt2; - pt2 = tmp; -} -//------------------------------------------------------------------------------ - -bool GetOverlapSegment(IntPoint pt1a, IntPoint pt1b, IntPoint pt2a, - IntPoint pt2b, IntPoint &pt1, IntPoint &pt2) -{ - //precondition: segments are Collinear. - if (Abs(pt1a.X - pt1b.X) > Abs(pt1a.Y - pt1b.Y)) - { - if (pt1a.X > pt1b.X) SwapPoints(pt1a, pt1b); - if (pt2a.X > pt2b.X) SwapPoints(pt2a, pt2b); - if (pt1a.X > pt2a.X) pt1 = pt1a; else pt1 = pt2a; - if (pt1b.X < pt2b.X) pt2 = pt1b; else pt2 = pt2b; - return pt1.X < pt2.X; - } else - { - if (pt1a.Y < pt1b.Y) SwapPoints(pt1a, pt1b); - if (pt2a.Y < pt2b.Y) SwapPoints(pt2a, pt2b); - if (pt1a.Y < pt2a.Y) pt1 = pt1a; else pt1 = pt2a; - if (pt1b.Y > pt2b.Y) pt2 = pt1b; else pt2 = pt2b; - return pt1.Y > pt2.Y; - } -} -//------------------------------------------------------------------------------ - -bool FirstIsBottomPt(const OutPt* btmPt1, const OutPt* btmPt2) -{ - OutPt *p = btmPt1->Prev; - while ((p->Pt == btmPt1->Pt) && (p != btmPt1)) p = p->Prev; - double dx1p = std::fabs(GetDx(btmPt1->Pt, p->Pt)); - p = btmPt1->Next; - while ((p->Pt == btmPt1->Pt) && (p != btmPt1)) p = p->Next; - double dx1n = std::fabs(GetDx(btmPt1->Pt, p->Pt)); - - p = btmPt2->Prev; - while ((p->Pt == btmPt2->Pt) && (p != btmPt2)) p = p->Prev; - double dx2p = std::fabs(GetDx(btmPt2->Pt, p->Pt)); - p = btmPt2->Next; - while ((p->Pt == btmPt2->Pt) && (p != btmPt2)) p = p->Next; - double dx2n = std::fabs(GetDx(btmPt2->Pt, p->Pt)); - - if (std::max(dx1p, dx1n) == std::max(dx2p, dx2n) && - std::min(dx1p, dx1n) == std::min(dx2p, dx2n)) - return Area(btmPt1) > 0; //if otherwise identical use orientation - else - return (dx1p >= dx2p && dx1p >= dx2n) || (dx1n >= dx2p && dx1n >= dx2n); -} -//------------------------------------------------------------------------------ - -OutPt* GetBottomPt(OutPt *pp) -{ - OutPt* dups = 0; - OutPt* p = pp->Next; - while (p != pp) - { - if (p->Pt.Y > pp->Pt.Y) - { - pp = p; - dups = 0; - } - else if (p->Pt.Y == pp->Pt.Y && p->Pt.X <= pp->Pt.X) - { - if (p->Pt.X < pp->Pt.X) - { - dups = 0; - pp = p; - } else - { - if (p->Next != pp && p->Prev != pp) dups = p; - } - } - p = p->Next; - } - if (dups) - { - //there appears to be at least 2 vertices at BottomPt so ... - while (dups != p) - { - if (!FirstIsBottomPt(p, dups)) pp = dups; - dups = dups->Next; - while (dups->Pt != pp->Pt) dups = dups->Next; - } - } - return pp; -} -//------------------------------------------------------------------------------ - -bool Pt2IsBetweenPt1AndPt3(const IntPoint pt1, - const IntPoint pt2, const IntPoint pt3) -{ - if ((pt1 == pt3) || (pt1 == pt2) || (pt3 == pt2)) - return false; - else if (pt1.X != pt3.X) - return (pt2.X > pt1.X) == (pt2.X < pt3.X); - else - return (pt2.Y > pt1.Y) == (pt2.Y < pt3.Y); -} -//------------------------------------------------------------------------------ - -bool HorzSegmentsOverlap(cInt seg1a, cInt seg1b, cInt seg2a, cInt seg2b) -{ - if (seg1a > seg1b) std::swap(seg1a, seg1b); - if (seg2a > seg2b) std::swap(seg2a, seg2b); - return (seg1a < seg2b) && (seg2a < seg1b); -} - -//------------------------------------------------------------------------------ -// ClipperBase class methods ... -//------------------------------------------------------------------------------ - -ClipperBase::ClipperBase() //constructor -{ - m_CurrentLM = m_MinimaList.begin(); //begin() == end() here - m_UseFullRange = false; -} -//------------------------------------------------------------------------------ - -ClipperBase::~ClipperBase() //destructor -{ - Clear(); -} -//------------------------------------------------------------------------------ - -void RangeTest(const IntPoint& Pt, bool& useFullRange) -{ - if (useFullRange) - { - if (Pt.X > hiRange || Pt.Y > hiRange || -Pt.X > hiRange || -Pt.Y > hiRange) - throw clipperException("Coordinate outside allowed range"); - } - else if (Pt.X > loRange|| Pt.Y > loRange || -Pt.X > loRange || -Pt.Y > loRange) - { - useFullRange = true; - RangeTest(Pt, useFullRange); - } -} -//------------------------------------------------------------------------------ - -TEdge* FindNextLocMin(TEdge* E) -{ - for (;;) - { - while (E->Bot != E->Prev->Bot || E->Curr == E->Top) E = E->Next; - if (!IsHorizontal(*E) && !IsHorizontal(*E->Prev)) break; - while (IsHorizontal(*E->Prev)) E = E->Prev; - TEdge* E2 = E; - while (IsHorizontal(*E)) E = E->Next; - if (E->Top.Y == E->Prev->Bot.Y) continue; //ie just an intermediate horz. - if (E2->Prev->Bot.X < E->Bot.X) E = E2; - break; - } - return E; -} -//------------------------------------------------------------------------------ - -TEdge* ClipperBase::ProcessBound(TEdge* E, bool NextIsForward) -{ - TEdge *Result = E; - TEdge *Horz = 0; - - if (E->OutIdx == Skip) - { - //if edges still remain in the current bound beyond the skip edge then - //create another LocMin and call ProcessBound once more - if (NextIsForward) - { - while (E->Top.Y == E->Next->Bot.Y) E = E->Next; - //don't include top horizontals when parsing a bound a second time, - //they will be contained in the opposite bound ... - while (E != Result && IsHorizontal(*E)) E = E->Prev; - } - else - { - while (E->Top.Y == E->Prev->Bot.Y) E = E->Prev; - while (E != Result && IsHorizontal(*E)) E = E->Next; - } - - if (E == Result) - { - if (NextIsForward) Result = E->Next; - else Result = E->Prev; - } - else - { - //there are more edges in the bound beyond result starting with E - if (NextIsForward) - E = Result->Next; - else - E = Result->Prev; - MinimaList::value_type locMin; - locMin.Y = E->Bot.Y; - locMin.LeftBound = 0; - locMin.RightBound = E; - E->WindDelta = 0; - Result = ProcessBound(E, NextIsForward); - m_MinimaList.push_back(locMin); - } - return Result; - } - - TEdge *EStart; - - if (IsHorizontal(*E)) - { - //We need to be careful with open paths because this may not be a - //true local minima (ie E may be following a skip edge). - //Also, consecutive horz. edges may start heading left before going right. - if (NextIsForward) - EStart = E->Prev; - else - EStart = E->Next; - if (IsHorizontal(*EStart)) //ie an adjoining horizontal skip edge - { - if (EStart->Bot.X != E->Bot.X && EStart->Top.X != E->Bot.X) - ReverseHorizontal(*E); - } - else if (EStart->Bot.X != E->Bot.X) - ReverseHorizontal(*E); - } - - EStart = E; - if (NextIsForward) - { - while (Result->Top.Y == Result->Next->Bot.Y && Result->Next->OutIdx != Skip) - Result = Result->Next; - if (IsHorizontal(*Result) && Result->Next->OutIdx != Skip) - { - //nb: at the top of a bound, horizontals are added to the bound - //only when the preceding edge attaches to the horizontal's left vertex - //unless a Skip edge is encountered when that becomes the top divide - Horz = Result; - while (IsHorizontal(*Horz->Prev)) Horz = Horz->Prev; - if (Horz->Prev->Top.X > Result->Next->Top.X) Result = Horz->Prev; - } - while (E != Result) - { - E->NextInLML = E->Next; - if (IsHorizontal(*E) && E != EStart && - E->Bot.X != E->Prev->Top.X) ReverseHorizontal(*E); - E = E->Next; - } - if (IsHorizontal(*E) && E != EStart && E->Bot.X != E->Prev->Top.X) - ReverseHorizontal(*E); - Result = Result->Next; //move to the edge just beyond current bound - } else - { - while (Result->Top.Y == Result->Prev->Bot.Y && Result->Prev->OutIdx != Skip) - Result = Result->Prev; - if (IsHorizontal(*Result) && Result->Prev->OutIdx != Skip) - { - Horz = Result; - while (IsHorizontal(*Horz->Next)) Horz = Horz->Next; - if (Horz->Next->Top.X == Result->Prev->Top.X || - Horz->Next->Top.X > Result->Prev->Top.X) Result = Horz->Next; - } - - while (E != Result) - { - E->NextInLML = E->Prev; - if (IsHorizontal(*E) && E != EStart && E->Bot.X != E->Next->Top.X) - ReverseHorizontal(*E); - E = E->Prev; - } - if (IsHorizontal(*E) && E != EStart && E->Bot.X != E->Next->Top.X) - ReverseHorizontal(*E); - Result = Result->Prev; //move to the edge just beyond current bound - } - - return Result; -} -//------------------------------------------------------------------------------ - -bool ClipperBase::AddPath(const Path &pg, PolyType PolyTyp, bool Closed) -{ -#ifdef use_lines - if (!Closed && PolyTyp == ptClip) - throw clipperException("AddPath: Open paths must be subject."); -#else - if (!Closed) - throw clipperException("AddPath: Open paths have been disabled."); -#endif - - int highI = (int)pg.size() -1; - if (Closed) while (highI > 0 && (pg[highI] == pg[0])) --highI; - while (highI > 0 && (pg[highI] == pg[highI -1])) --highI; - if ((Closed && highI < 2) || (!Closed && highI < 1)) return false; - - //create a new edge array ... - TEdge *edges = new TEdge [highI +1]; - - bool IsFlat = true; - //1. Basic (first) edge initialization ... - try - { - edges[1].Curr = pg[1]; - RangeTest(pg[0], m_UseFullRange); - RangeTest(pg[highI], m_UseFullRange); - InitEdge(&edges[0], &edges[1], &edges[highI], pg[0]); - InitEdge(&edges[highI], &edges[0], &edges[highI-1], pg[highI]); - for (int i = highI - 1; i >= 1; --i) - { - RangeTest(pg[i], m_UseFullRange); - InitEdge(&edges[i], &edges[i+1], &edges[i-1], pg[i]); - } - } - catch(...) - { - delete [] edges; - throw; //range test fails - } - TEdge *eStart = &edges[0]; - - //2. Remove duplicate vertices, and (when closed) collinear edges ... - TEdge *E = eStart, *eLoopStop = eStart; - for (;;) - { - //nb: allows matching start and end points when not Closed ... - if (E->Curr == E->Next->Curr && (Closed || E->Next != eStart)) - { - if (E == E->Next) break; - if (E == eStart) eStart = E->Next; - E = RemoveEdge(E); - eLoopStop = E; - continue; - } - if (E->Prev == E->Next) - break; //only two vertices - else if (Closed && - SlopesEqual(E->Prev->Curr, E->Curr, E->Next->Curr, m_UseFullRange) && - (!m_PreserveCollinear || - !Pt2IsBetweenPt1AndPt3(E->Prev->Curr, E->Curr, E->Next->Curr))) - { - //Collinear edges are allowed for open paths but in closed paths - //the default is to merge adjacent collinear edges into a single edge. - //However, if the PreserveCollinear property is enabled, only overlapping - //collinear edges (ie spikes) will be removed from closed paths. - if (E == eStart) eStart = E->Next; - E = RemoveEdge(E); - E = E->Prev; - eLoopStop = E; - continue; - } - E = E->Next; - if ((E == eLoopStop) || (!Closed && E->Next == eStart)) break; - } - - if ((!Closed && (E == E->Next)) || (Closed && (E->Prev == E->Next))) - { - delete [] edges; - return false; - } - - if (!Closed) - { - m_HasOpenPaths = true; - eStart->Prev->OutIdx = Skip; - } - - //3. Do second stage of edge initialization ... - E = eStart; - do - { - InitEdge2(*E, PolyTyp); - E = E->Next; - if (IsFlat && E->Curr.Y != eStart->Curr.Y) IsFlat = false; - } - while (E != eStart); - - //4. Finally, add edge bounds to LocalMinima list ... - - //Totally flat paths must be handled differently when adding them - //to LocalMinima list to avoid endless loops etc ... - if (IsFlat) - { - if (Closed) - { - delete [] edges; - return false; - } - E->Prev->OutIdx = Skip; - MinimaList::value_type locMin; - locMin.Y = E->Bot.Y; - locMin.LeftBound = 0; - locMin.RightBound = E; - locMin.RightBound->Side = esRight; - locMin.RightBound->WindDelta = 0; - for (;;) - { - if (E->Bot.X != E->Prev->Top.X) ReverseHorizontal(*E); - if (E->Next->OutIdx == Skip) break; - E->NextInLML = E->Next; - E = E->Next; - } - m_MinimaList.push_back(locMin); - m_edges.push_back(edges); - return true; - } - - m_edges.push_back(edges); - bool leftBoundIsForward; - TEdge* EMin = 0; - - //workaround to avoid an endless loop in the while loop below when - //open paths have matching start and end points ... - if (E->Prev->Bot == E->Prev->Top) E = E->Next; - - for (;;) - { - E = FindNextLocMin(E); - if (E == EMin) break; - else if (!EMin) EMin = E; - - //E and E.Prev now share a local minima (left aligned if horizontal). - //Compare their slopes to find which starts which bound ... - MinimaList::value_type locMin; - locMin.Y = E->Bot.Y; - if (E->Dx < E->Prev->Dx) - { - locMin.LeftBound = E->Prev; - locMin.RightBound = E; - leftBoundIsForward = false; //Q.nextInLML = Q.prev - } else - { - locMin.LeftBound = E; - locMin.RightBound = E->Prev; - leftBoundIsForward = true; //Q.nextInLML = Q.next - } - - if (!Closed) locMin.LeftBound->WindDelta = 0; - else if (locMin.LeftBound->Next == locMin.RightBound) - locMin.LeftBound->WindDelta = -1; - else locMin.LeftBound->WindDelta = 1; - locMin.RightBound->WindDelta = -locMin.LeftBound->WindDelta; - - E = ProcessBound(locMin.LeftBound, leftBoundIsForward); - if (E->OutIdx == Skip) E = ProcessBound(E, leftBoundIsForward); - - TEdge* E2 = ProcessBound(locMin.RightBound, !leftBoundIsForward); - if (E2->OutIdx == Skip) E2 = ProcessBound(E2, !leftBoundIsForward); - - if (locMin.LeftBound->OutIdx == Skip) - locMin.LeftBound = 0; - else if (locMin.RightBound->OutIdx == Skip) - locMin.RightBound = 0; - m_MinimaList.push_back(locMin); - if (!leftBoundIsForward) E = E2; - } - return true; -} -//------------------------------------------------------------------------------ - -bool ClipperBase::AddPaths(const Paths &ppg, PolyType PolyTyp, bool Closed) -{ - bool result = false; - for (Paths::size_type i = 0; i < ppg.size(); ++i) - if (AddPath(ppg[i], PolyTyp, Closed)) result = true; - return result; -} -//------------------------------------------------------------------------------ - -void ClipperBase::Clear() -{ - DisposeLocalMinimaList(); - for (EdgeList::size_type i = 0; i < m_edges.size(); ++i) - { - TEdge* edges = m_edges[i]; - delete [] edges; - } - m_edges.clear(); - m_UseFullRange = false; - m_HasOpenPaths = false; -} -//------------------------------------------------------------------------------ - -void ClipperBase::Reset() -{ - m_CurrentLM = m_MinimaList.begin(); - if (m_CurrentLM == m_MinimaList.end()) return; //ie nothing to process - std::sort(m_MinimaList.begin(), m_MinimaList.end(), LocMinSorter()); - - m_Scanbeam = ScanbeamList(); //clears/resets priority_queue - //reset all edges ... - for (MinimaList::iterator lm = m_MinimaList.begin(); lm != m_MinimaList.end(); ++lm) - { - InsertScanbeam(lm->Y); - TEdge* e = lm->LeftBound; - if (e) - { - e->Curr = e->Bot; - e->Side = esLeft; - e->OutIdx = Unassigned; - } - - e = lm->RightBound; - if (e) - { - e->Curr = e->Bot; - e->Side = esRight; - e->OutIdx = Unassigned; - } - } - m_ActiveEdges = 0; - m_CurrentLM = m_MinimaList.begin(); -} -//------------------------------------------------------------------------------ - -void ClipperBase::DisposeLocalMinimaList() -{ - m_MinimaList.clear(); - m_CurrentLM = m_MinimaList.begin(); -} -//------------------------------------------------------------------------------ - -bool ClipperBase::PopLocalMinima(cInt Y, const LocalMinimum *&locMin) -{ - if (m_CurrentLM == m_MinimaList.end() || (*m_CurrentLM).Y != Y) return false; - locMin = &(*m_CurrentLM); - ++m_CurrentLM; - return true; -} -//------------------------------------------------------------------------------ - -IntRect ClipperBase::GetBounds() -{ - IntRect result; - MinimaList::iterator lm = m_MinimaList.begin(); - if (lm == m_MinimaList.end()) - { - result.left = result.top = result.right = result.bottom = 0; - return result; - } - result.left = lm->LeftBound->Bot.X; - result.top = lm->LeftBound->Bot.Y; - result.right = lm->LeftBound->Bot.X; - result.bottom = lm->LeftBound->Bot.Y; - while (lm != m_MinimaList.end()) - { - //todo - needs fixing for open paths - result.bottom = std::max(result.bottom, lm->LeftBound->Bot.Y); - TEdge* e = lm->LeftBound; - for (;;) { - TEdge* bottomE = e; - while (e->NextInLML) - { - if (e->Bot.X < result.left) result.left = e->Bot.X; - if (e->Bot.X > result.right) result.right = e->Bot.X; - e = e->NextInLML; - } - result.left = std::min(result.left, e->Bot.X); - result.right = std::max(result.right, e->Bot.X); - result.left = std::min(result.left, e->Top.X); - result.right = std::max(result.right, e->Top.X); - result.top = std::min(result.top, e->Top.Y); - if (bottomE == lm->LeftBound) e = lm->RightBound; - else break; - } - ++lm; - } - return result; -} -//------------------------------------------------------------------------------ - -void ClipperBase::InsertScanbeam(const cInt Y) -{ - m_Scanbeam.push(Y); -} -//------------------------------------------------------------------------------ - -bool ClipperBase::PopScanbeam(cInt &Y) -{ - if (m_Scanbeam.empty()) return false; - Y = m_Scanbeam.top(); - m_Scanbeam.pop(); - while (!m_Scanbeam.empty() && Y == m_Scanbeam.top()) { m_Scanbeam.pop(); } // Pop duplicates. - return true; -} -//------------------------------------------------------------------------------ - -void ClipperBase::DisposeAllOutRecs(){ - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i) - DisposeOutRec(i); - m_PolyOuts.clear(); -} -//------------------------------------------------------------------------------ - -void ClipperBase::DisposeOutRec(PolyOutList::size_type index) -{ - OutRec *outRec = m_PolyOuts[index]; - if (outRec->Pts) DisposeOutPts(outRec->Pts); - delete outRec; - m_PolyOuts[index] = 0; -} -//------------------------------------------------------------------------------ - -void ClipperBase::DeleteFromAEL(TEdge *e) -{ - TEdge* AelPrev = e->PrevInAEL; - TEdge* AelNext = e->NextInAEL; - if (!AelPrev && !AelNext && (e != m_ActiveEdges)) return; //already deleted - if (AelPrev) AelPrev->NextInAEL = AelNext; - else m_ActiveEdges = AelNext; - if (AelNext) AelNext->PrevInAEL = AelPrev; - e->NextInAEL = 0; - e->PrevInAEL = 0; -} -//------------------------------------------------------------------------------ - -OutRec* ClipperBase::CreateOutRec() -{ - OutRec* result = new OutRec; - result->IsHole = false; - result->IsOpen = false; - result->FirstLeft = 0; - result->Pts = 0; - result->BottomPt = 0; - result->PolyNd = 0; - m_PolyOuts.push_back(result); - result->Idx = (int)m_PolyOuts.size() - 1; - return result; -} -//------------------------------------------------------------------------------ - -void ClipperBase::SwapPositionsInAEL(TEdge *Edge1, TEdge *Edge2) -{ - //check that one or other edge hasn't already been removed from AEL ... - if (Edge1->NextInAEL == Edge1->PrevInAEL || - Edge2->NextInAEL == Edge2->PrevInAEL) return; - - if (Edge1->NextInAEL == Edge2) - { - TEdge* Next = Edge2->NextInAEL; - if (Next) Next->PrevInAEL = Edge1; - TEdge* Prev = Edge1->PrevInAEL; - if (Prev) Prev->NextInAEL = Edge2; - Edge2->PrevInAEL = Prev; - Edge2->NextInAEL = Edge1; - Edge1->PrevInAEL = Edge2; - Edge1->NextInAEL = Next; - } - else if (Edge2->NextInAEL == Edge1) - { - TEdge* Next = Edge1->NextInAEL; - if (Next) Next->PrevInAEL = Edge2; - TEdge* Prev = Edge2->PrevInAEL; - if (Prev) Prev->NextInAEL = Edge1; - Edge1->PrevInAEL = Prev; - Edge1->NextInAEL = Edge2; - Edge2->PrevInAEL = Edge1; - Edge2->NextInAEL = Next; - } - else - { - TEdge* Next = Edge1->NextInAEL; - TEdge* Prev = Edge1->PrevInAEL; - Edge1->NextInAEL = Edge2->NextInAEL; - if (Edge1->NextInAEL) Edge1->NextInAEL->PrevInAEL = Edge1; - Edge1->PrevInAEL = Edge2->PrevInAEL; - if (Edge1->PrevInAEL) Edge1->PrevInAEL->NextInAEL = Edge1; - Edge2->NextInAEL = Next; - if (Edge2->NextInAEL) Edge2->NextInAEL->PrevInAEL = Edge2; - Edge2->PrevInAEL = Prev; - if (Edge2->PrevInAEL) Edge2->PrevInAEL->NextInAEL = Edge2; - } - - if (!Edge1->PrevInAEL) m_ActiveEdges = Edge1; - else if (!Edge2->PrevInAEL) m_ActiveEdges = Edge2; -} -//------------------------------------------------------------------------------ - -void ClipperBase::UpdateEdgeIntoAEL(TEdge *&e) -{ - if (!e->NextInLML) - throw clipperException("UpdateEdgeIntoAEL: invalid call"); - - e->NextInLML->OutIdx = e->OutIdx; - TEdge* AelPrev = e->PrevInAEL; - TEdge* AelNext = e->NextInAEL; - if (AelPrev) AelPrev->NextInAEL = e->NextInLML; - else m_ActiveEdges = e->NextInLML; - if (AelNext) AelNext->PrevInAEL = e->NextInLML; - e->NextInLML->Side = e->Side; - e->NextInLML->WindDelta = e->WindDelta; - e->NextInLML->WindCnt = e->WindCnt; - e->NextInLML->WindCnt2 = e->WindCnt2; - e = e->NextInLML; - e->Curr = e->Bot; - e->PrevInAEL = AelPrev; - e->NextInAEL = AelNext; - if (!IsHorizontal(*e)) InsertScanbeam(e->Top.Y); -} -//------------------------------------------------------------------------------ - -bool ClipperBase::LocalMinimaPending() -{ - return (m_CurrentLM != m_MinimaList.end()); -} - -//------------------------------------------------------------------------------ -// TClipper methods ... -//------------------------------------------------------------------------------ - -Clipper::Clipper(int initOptions) : ClipperBase() //constructor -{ - m_ExecuteLocked = false; - m_UseFullRange = false; - m_ReverseOutput = ((initOptions & ioReverseSolution) != 0); - m_StrictSimple = ((initOptions & ioStrictlySimple) != 0); - m_PreserveCollinear = ((initOptions & ioPreserveCollinear) != 0); - m_HasOpenPaths = false; -#ifdef use_xyz - m_ZFill = 0; -#endif -} -//------------------------------------------------------------------------------ - -#ifdef use_xyz -void Clipper::ZFillFunction(ZFillCallback zFillFunc) -{ - m_ZFill = zFillFunc; -} -//------------------------------------------------------------------------------ -#endif - -bool Clipper::Execute(ClipType clipType, Paths &solution, PolyFillType fillType) -{ - return Execute(clipType, solution, fillType, fillType); -} -//------------------------------------------------------------------------------ - -bool Clipper::Execute(ClipType clipType, PolyTree &polytree, PolyFillType fillType) -{ - return Execute(clipType, polytree, fillType, fillType); -} -//------------------------------------------------------------------------------ - -bool Clipper::Execute(ClipType clipType, Paths &solution, - PolyFillType subjFillType, PolyFillType clipFillType) -{ - if( m_ExecuteLocked ) return false; - if (m_HasOpenPaths) - throw clipperException("Error: PolyTree struct is needed for open path clipping."); - m_ExecuteLocked = true; - solution.resize(0); - m_SubjFillType = subjFillType; - m_ClipFillType = clipFillType; - m_ClipType = clipType; - m_UsingPolyTree = false; - bool succeeded = ExecuteInternal(); - if (succeeded) BuildResult(solution); - DisposeAllOutRecs(); - m_ExecuteLocked = false; - return succeeded; -} -//------------------------------------------------------------------------------ - -bool Clipper::Execute(ClipType clipType, PolyTree& polytree, - PolyFillType subjFillType, PolyFillType clipFillType) -{ - if( m_ExecuteLocked ) return false; - m_ExecuteLocked = true; - m_SubjFillType = subjFillType; - m_ClipFillType = clipFillType; - m_ClipType = clipType; - m_UsingPolyTree = true; - bool succeeded = ExecuteInternal(); - if (succeeded) BuildResult2(polytree); - DisposeAllOutRecs(); - m_ExecuteLocked = false; - return succeeded; -} -//------------------------------------------------------------------------------ - -void Clipper::FixHoleLinkage(OutRec &outrec) -{ - //skip OutRecs that (a) contain outermost polygons or - //(b) already have the correct owner/child linkage ... - if (!outrec.FirstLeft || - (outrec.IsHole != outrec.FirstLeft->IsHole && - outrec.FirstLeft->Pts)) return; - - OutRec* orfl = outrec.FirstLeft; - while (orfl && ((orfl->IsHole == outrec.IsHole) || !orfl->Pts)) - orfl = orfl->FirstLeft; - outrec.FirstLeft = orfl; -} -//------------------------------------------------------------------------------ - -bool Clipper::ExecuteInternal() -{ - bool succeeded = true; - try { - Reset(); - m_Maxima = MaximaList(); - m_SortedEdges = 0; - - succeeded = true; - cInt botY, topY; - if (!PopScanbeam(botY)) return false; - InsertLocalMinimaIntoAEL(botY); - while (PopScanbeam(topY) || LocalMinimaPending()) - { - ProcessHorizontals(); - ClearGhostJoins(); - if (!ProcessIntersections(topY)) - { - succeeded = false; - break; - } - ProcessEdgesAtTopOfScanbeam(topY); - botY = topY; - InsertLocalMinimaIntoAEL(botY); - } - } - catch(...) - { - succeeded = false; - } - - if (succeeded) - { - //fix orientations ... - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i) - { - OutRec *outRec = m_PolyOuts[i]; - if (!outRec->Pts || outRec->IsOpen) continue; - if ((outRec->IsHole ^ m_ReverseOutput) == (Area(*outRec) > 0)) - ReversePolyPtLinks(outRec->Pts); - } - - if (!m_Joins.empty()) JoinCommonEdges(); - - //unfortunately FixupOutPolygon() must be done after JoinCommonEdges() - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i) - { - OutRec *outRec = m_PolyOuts[i]; - if (!outRec->Pts) continue; - if (outRec->IsOpen) - FixupOutPolyline(*outRec); - else - FixupOutPolygon(*outRec); - } - - if (m_StrictSimple) DoSimplePolygons(); - } - - ClearJoins(); - ClearGhostJoins(); - return succeeded; -} -//------------------------------------------------------------------------------ - -void Clipper::SetWindingCount(TEdge &edge) -{ - TEdge *e = edge.PrevInAEL; - //find the edge of the same polytype that immediately preceeds 'edge' in AEL - while (e && ((e->PolyTyp != edge.PolyTyp) || (e->WindDelta == 0))) e = e->PrevInAEL; - if (!e) - { - if (edge.WindDelta == 0) - { - PolyFillType pft = (edge.PolyTyp == ptSubject ? m_SubjFillType : m_ClipFillType); - edge.WindCnt = (pft == pftNegative ? -1 : 1); - } - else - edge.WindCnt = edge.WindDelta; - edge.WindCnt2 = 0; - e = m_ActiveEdges; //ie get ready to calc WindCnt2 - } - else if (edge.WindDelta == 0 && m_ClipType != ctUnion) - { - edge.WindCnt = 1; - edge.WindCnt2 = e->WindCnt2; - e = e->NextInAEL; //ie get ready to calc WindCnt2 - } - else if (IsEvenOddFillType(edge)) - { - //EvenOdd filling ... - if (edge.WindDelta == 0) - { - //are we inside a subj polygon ... - bool Inside = true; - TEdge *e2 = e->PrevInAEL; - while (e2) - { - if (e2->PolyTyp == e->PolyTyp && e2->WindDelta != 0) - Inside = !Inside; - e2 = e2->PrevInAEL; - } - edge.WindCnt = (Inside ? 0 : 1); - } - else - { - edge.WindCnt = edge.WindDelta; - } - edge.WindCnt2 = e->WindCnt2; - e = e->NextInAEL; //ie get ready to calc WindCnt2 - } - else - { - //nonZero, Positive or Negative filling ... - if (e->WindCnt * e->WindDelta < 0) - { - //prev edge is 'decreasing' WindCount (WC) toward zero - //so we're outside the previous polygon ... - if (Abs(e->WindCnt) > 1) - { - //outside prev poly but still inside another. - //when reversing direction of prev poly use the same WC - if (e->WindDelta * edge.WindDelta < 0) edge.WindCnt = e->WindCnt; - //otherwise continue to 'decrease' WC ... - else edge.WindCnt = e->WindCnt + edge.WindDelta; - } - else - //now outside all polys of same polytype so set own WC ... - edge.WindCnt = (edge.WindDelta == 0 ? 1 : edge.WindDelta); - } else - { - //prev edge is 'increasing' WindCount (WC) away from zero - //so we're inside the previous polygon ... - if (edge.WindDelta == 0) - edge.WindCnt = (e->WindCnt < 0 ? e->WindCnt - 1 : e->WindCnt + 1); - //if wind direction is reversing prev then use same WC - else if (e->WindDelta * edge.WindDelta < 0) edge.WindCnt = e->WindCnt; - //otherwise add to WC ... - else edge.WindCnt = e->WindCnt + edge.WindDelta; - } - edge.WindCnt2 = e->WindCnt2; - e = e->NextInAEL; //ie get ready to calc WindCnt2 - } - - //update WindCnt2 ... - if (IsEvenOddAltFillType(edge)) - { - //EvenOdd filling ... - while (e != &edge) - { - if (e->WindDelta != 0) - edge.WindCnt2 = (edge.WindCnt2 == 0 ? 1 : 0); - e = e->NextInAEL; - } - } else - { - //nonZero, Positive or Negative filling ... - while ( e != &edge ) - { - edge.WindCnt2 += e->WindDelta; - e = e->NextInAEL; - } - } -} -//------------------------------------------------------------------------------ - -bool Clipper::IsEvenOddFillType(const TEdge& edge) const -{ - if (edge.PolyTyp == ptSubject) - return m_SubjFillType == pftEvenOdd; else - return m_ClipFillType == pftEvenOdd; -} -//------------------------------------------------------------------------------ - -bool Clipper::IsEvenOddAltFillType(const TEdge& edge) const -{ - if (edge.PolyTyp == ptSubject) - return m_ClipFillType == pftEvenOdd; else - return m_SubjFillType == pftEvenOdd; -} -//------------------------------------------------------------------------------ - -bool Clipper::IsContributing(const TEdge& edge) const -{ - PolyFillType pft, pft2; - if (edge.PolyTyp == ptSubject) - { - pft = m_SubjFillType; - pft2 = m_ClipFillType; - } else - { - pft = m_ClipFillType; - pft2 = m_SubjFillType; - } - - switch(pft) - { - case pftEvenOdd: - //return false if a subj line has been flagged as inside a subj polygon - if (edge.WindDelta == 0 && edge.WindCnt != 1) return false; - break; - case pftNonZero: - if (Abs(edge.WindCnt) != 1) return false; - break; - case pftPositive: - if (edge.WindCnt != 1) return false; - break; - default: //pftNegative - if (edge.WindCnt != -1) return false; - } - - switch(m_ClipType) - { - case ctIntersection: - switch(pft2) - { - case pftEvenOdd: - case pftNonZero: - return (edge.WindCnt2 != 0); - case pftPositive: - return (edge.WindCnt2 > 0); - default: - return (edge.WindCnt2 < 0); - } - break; - case ctUnion: - switch(pft2) - { - case pftEvenOdd: - case pftNonZero: - return (edge.WindCnt2 == 0); - case pftPositive: - return (edge.WindCnt2 <= 0); - default: - return (edge.WindCnt2 >= 0); - } - break; - case ctDifference: - if (edge.PolyTyp == ptSubject) - switch(pft2) - { - case pftEvenOdd: - case pftNonZero: - return (edge.WindCnt2 == 0); - case pftPositive: - return (edge.WindCnt2 <= 0); - default: - return (edge.WindCnt2 >= 0); - } - else - switch(pft2) - { - case pftEvenOdd: - case pftNonZero: - return (edge.WindCnt2 != 0); - case pftPositive: - return (edge.WindCnt2 > 0); - default: - return (edge.WindCnt2 < 0); - } - break; - case ctXor: - if (edge.WindDelta == 0) //XOr always contributing unless open - switch(pft2) - { - case pftEvenOdd: - case pftNonZero: - return (edge.WindCnt2 == 0); - case pftPositive: - return (edge.WindCnt2 <= 0); - default: - return (edge.WindCnt2 >= 0); - } - else - return true; - break; - default: - return true; - } -} -//------------------------------------------------------------------------------ - -OutPt* Clipper::AddLocalMinPoly(TEdge *e1, TEdge *e2, const IntPoint &Pt) -{ - OutPt* result; - TEdge *e, *prevE; - if (IsHorizontal(*e2) || ( e1->Dx > e2->Dx )) - { - result = AddOutPt(e1, Pt); - e2->OutIdx = e1->OutIdx; - e1->Side = esLeft; - e2->Side = esRight; - e = e1; - if (e->PrevInAEL == e2) - prevE = e2->PrevInAEL; - else - prevE = e->PrevInAEL; - } else - { - result = AddOutPt(e2, Pt); - e1->OutIdx = e2->OutIdx; - e1->Side = esRight; - e2->Side = esLeft; - e = e2; - if (e->PrevInAEL == e1) - prevE = e1->PrevInAEL; - else - prevE = e->PrevInAEL; - } - - if (prevE && prevE->OutIdx >= 0 && prevE->Top.Y < Pt.Y && e->Top.Y < Pt.Y) - { - cInt xPrev = TopX(*prevE, Pt.Y); - cInt xE = TopX(*e, Pt.Y); - if (xPrev == xE && (e->WindDelta != 0) && (prevE->WindDelta != 0) && - SlopesEqual(IntPoint(xPrev, Pt.Y), prevE->Top, IntPoint(xE, Pt.Y), e->Top, m_UseFullRange)) - { - OutPt* outPt = AddOutPt(prevE, Pt); - AddJoin(result, outPt, e->Top); - } - } - return result; -} -//------------------------------------------------------------------------------ - -void Clipper::AddLocalMaxPoly(TEdge *e1, TEdge *e2, const IntPoint &Pt) -{ - AddOutPt( e1, Pt ); - if (e2->WindDelta == 0) AddOutPt(e2, Pt); - if( e1->OutIdx == e2->OutIdx ) - { - e1->OutIdx = Unassigned; - e2->OutIdx = Unassigned; - } - else if (e1->OutIdx < e2->OutIdx) - AppendPolygon(e1, e2); - else - AppendPolygon(e2, e1); -} -//------------------------------------------------------------------------------ - -void Clipper::AddEdgeToSEL(TEdge *edge) -{ - //SEL pointers in PEdge are reused to build a list of horizontal edges. - //However, we don't need to worry about order with horizontal edge processing. - if( !m_SortedEdges ) - { - m_SortedEdges = edge; - edge->PrevInSEL = 0; - edge->NextInSEL = 0; - } - else - { - edge->NextInSEL = m_SortedEdges; - edge->PrevInSEL = 0; - m_SortedEdges->PrevInSEL = edge; - m_SortedEdges = edge; - } -} -//------------------------------------------------------------------------------ - -bool Clipper::PopEdgeFromSEL(TEdge *&edge) -{ - if (!m_SortedEdges) return false; - edge = m_SortedEdges; - DeleteFromSEL(m_SortedEdges); - return true; -} -//------------------------------------------------------------------------------ - -void Clipper::CopyAELToSEL() -{ - TEdge* e = m_ActiveEdges; - m_SortedEdges = e; - while ( e ) - { - e->PrevInSEL = e->PrevInAEL; - e->NextInSEL = e->NextInAEL; - e = e->NextInAEL; - } -} -//------------------------------------------------------------------------------ - -void Clipper::AddJoin(OutPt *op1, OutPt *op2, const IntPoint OffPt) -{ - Join* j = new Join; - j->OutPt1 = op1; - j->OutPt2 = op2; - j->OffPt = OffPt; - m_Joins.push_back(j); -} -//------------------------------------------------------------------------------ - -void Clipper::ClearJoins() -{ - for (JoinList::size_type i = 0; i < m_Joins.size(); i++) - delete m_Joins[i]; - m_Joins.resize(0); -} -//------------------------------------------------------------------------------ - -void Clipper::ClearGhostJoins() -{ - for (JoinList::size_type i = 0; i < m_GhostJoins.size(); i++) - delete m_GhostJoins[i]; - m_GhostJoins.resize(0); -} -//------------------------------------------------------------------------------ - -void Clipper::AddGhostJoin(OutPt *op, const IntPoint OffPt) -{ - Join* j = new Join; - j->OutPt1 = op; - j->OutPt2 = 0; - j->OffPt = OffPt; - m_GhostJoins.push_back(j); -} -//------------------------------------------------------------------------------ - -void Clipper::InsertLocalMinimaIntoAEL(const cInt botY) -{ - const LocalMinimum *lm; - while (PopLocalMinima(botY, lm)) - { - TEdge* lb = lm->LeftBound; - TEdge* rb = lm->RightBound; - - OutPt *Op1 = 0; - if (!lb) - { - //nb: don't insert LB into either AEL or SEL - InsertEdgeIntoAEL(rb, 0); - SetWindingCount(*rb); - if (IsContributing(*rb)) - Op1 = AddOutPt(rb, rb->Bot); - } - else if (!rb) - { - InsertEdgeIntoAEL(lb, 0); - SetWindingCount(*lb); - if (IsContributing(*lb)) - Op1 = AddOutPt(lb, lb->Bot); - InsertScanbeam(lb->Top.Y); - } - else - { - InsertEdgeIntoAEL(lb, 0); - InsertEdgeIntoAEL(rb, lb); - SetWindingCount( *lb ); - rb->WindCnt = lb->WindCnt; - rb->WindCnt2 = lb->WindCnt2; - if (IsContributing(*lb)) - Op1 = AddLocalMinPoly(lb, rb, lb->Bot); - InsertScanbeam(lb->Top.Y); - } - - if (rb) - { - if (IsHorizontal(*rb)) - { - AddEdgeToSEL(rb); - if (rb->NextInLML) - InsertScanbeam(rb->NextInLML->Top.Y); - } - else InsertScanbeam( rb->Top.Y ); - } - - if (!lb || !rb) continue; - - //if any output polygons share an edge, they'll need joining later ... - if (Op1 && IsHorizontal(*rb) && - m_GhostJoins.size() > 0 && (rb->WindDelta != 0)) - { - for (JoinList::size_type i = 0; i < m_GhostJoins.size(); ++i) - { - Join* jr = m_GhostJoins[i]; - //if the horizontal Rb and a 'ghost' horizontal overlap, then convert - //the 'ghost' join to a real join ready for later ... - if (HorzSegmentsOverlap(jr->OutPt1->Pt.X, jr->OffPt.X, rb->Bot.X, rb->Top.X)) - AddJoin(jr->OutPt1, Op1, jr->OffPt); - } - } - - if (lb->OutIdx >= 0 && lb->PrevInAEL && - lb->PrevInAEL->Curr.X == lb->Bot.X && - lb->PrevInAEL->OutIdx >= 0 && - SlopesEqual(lb->PrevInAEL->Bot, lb->PrevInAEL->Top, lb->Curr, lb->Top, m_UseFullRange) && - (lb->WindDelta != 0) && (lb->PrevInAEL->WindDelta != 0)) - { - OutPt *Op2 = AddOutPt(lb->PrevInAEL, lb->Bot); - AddJoin(Op1, Op2, lb->Top); - } - - if(lb->NextInAEL != rb) - { - - if (rb->OutIdx >= 0 && rb->PrevInAEL->OutIdx >= 0 && - SlopesEqual(rb->PrevInAEL->Curr, rb->PrevInAEL->Top, rb->Curr, rb->Top, m_UseFullRange) && - (rb->WindDelta != 0) && (rb->PrevInAEL->WindDelta != 0)) - { - OutPt *Op2 = AddOutPt(rb->PrevInAEL, rb->Bot); - AddJoin(Op1, Op2, rb->Top); - } - - TEdge* e = lb->NextInAEL; - if (e) - { - while( e != rb ) - { - //nb: For calculating winding counts etc, IntersectEdges() assumes - //that param1 will be to the Right of param2 ABOVE the intersection ... - IntersectEdges(rb , e , lb->Curr); //order important here - e = e->NextInAEL; - } - } - } - - } -} -//------------------------------------------------------------------------------ - -void Clipper::DeleteFromSEL(TEdge *e) -{ - TEdge* SelPrev = e->PrevInSEL; - TEdge* SelNext = e->NextInSEL; - if( !SelPrev && !SelNext && (e != m_SortedEdges) ) return; //already deleted - if( SelPrev ) SelPrev->NextInSEL = SelNext; - else m_SortedEdges = SelNext; - if( SelNext ) SelNext->PrevInSEL = SelPrev; - e->NextInSEL = 0; - e->PrevInSEL = 0; -} -//------------------------------------------------------------------------------ - -#ifdef use_xyz -void Clipper::SetZ(IntPoint& pt, TEdge& e1, TEdge& e2) -{ - if (pt.Z != 0 || !m_ZFill) return; - else if (pt == e1.Bot) pt.Z = e1.Bot.Z; - else if (pt == e1.Top) pt.Z = e1.Top.Z; - else if (pt == e2.Bot) pt.Z = e2.Bot.Z; - else if (pt == e2.Top) pt.Z = e2.Top.Z; - else (*m_ZFill)(e1.Bot, e1.Top, e2.Bot, e2.Top, pt); -} -//------------------------------------------------------------------------------ -#endif - -void Clipper::IntersectEdges(TEdge *e1, TEdge *e2, IntPoint &Pt) -{ - bool e1Contributing = ( e1->OutIdx >= 0 ); - bool e2Contributing = ( e2->OutIdx >= 0 ); - -#ifdef use_xyz - SetZ(Pt, *e1, *e2); -#endif - -#ifdef use_lines - //if either edge is on an OPEN path ... - if (e1->WindDelta == 0 || e2->WindDelta == 0) - { - //ignore subject-subject open path intersections UNLESS they - //are both open paths, AND they are both 'contributing maximas' ... - if (e1->WindDelta == 0 && e2->WindDelta == 0) return; - - //if intersecting a subj line with a subj poly ... - else if (e1->PolyTyp == e2->PolyTyp && - e1->WindDelta != e2->WindDelta && m_ClipType == ctUnion) - { - if (e1->WindDelta == 0) - { - if (e2Contributing) - { - AddOutPt(e1, Pt); - if (e1Contributing) e1->OutIdx = Unassigned; - } - } - else - { - if (e1Contributing) - { - AddOutPt(e2, Pt); - if (e2Contributing) e2->OutIdx = Unassigned; - } - } - } - else if (e1->PolyTyp != e2->PolyTyp) - { - //toggle subj open path OutIdx on/off when Abs(clip.WndCnt) == 1 ... - if ((e1->WindDelta == 0) && abs(e2->WindCnt) == 1 && - (m_ClipType != ctUnion || e2->WindCnt2 == 0)) - { - AddOutPt(e1, Pt); - if (e1Contributing) e1->OutIdx = Unassigned; - } - else if ((e2->WindDelta == 0) && (abs(e1->WindCnt) == 1) && - (m_ClipType != ctUnion || e1->WindCnt2 == 0)) - { - AddOutPt(e2, Pt); - if (e2Contributing) e2->OutIdx = Unassigned; - } - } - return; - } -#endif - - //update winding counts... - //assumes that e1 will be to the Right of e2 ABOVE the intersection - if ( e1->PolyTyp == e2->PolyTyp ) - { - if ( IsEvenOddFillType( *e1) ) - { - int oldE1WindCnt = e1->WindCnt; - e1->WindCnt = e2->WindCnt; - e2->WindCnt = oldE1WindCnt; - } else - { - if (e1->WindCnt + e2->WindDelta == 0 ) e1->WindCnt = -e1->WindCnt; - else e1->WindCnt += e2->WindDelta; - if ( e2->WindCnt - e1->WindDelta == 0 ) e2->WindCnt = -e2->WindCnt; - else e2->WindCnt -= e1->WindDelta; - } - } else - { - if (!IsEvenOddFillType(*e2)) e1->WindCnt2 += e2->WindDelta; - else e1->WindCnt2 = ( e1->WindCnt2 == 0 ) ? 1 : 0; - if (!IsEvenOddFillType(*e1)) e2->WindCnt2 -= e1->WindDelta; - else e2->WindCnt2 = ( e2->WindCnt2 == 0 ) ? 1 : 0; - } - - PolyFillType e1FillType, e2FillType, e1FillType2, e2FillType2; - if (e1->PolyTyp == ptSubject) - { - e1FillType = m_SubjFillType; - e1FillType2 = m_ClipFillType; - } else - { - e1FillType = m_ClipFillType; - e1FillType2 = m_SubjFillType; - } - if (e2->PolyTyp == ptSubject) - { - e2FillType = m_SubjFillType; - e2FillType2 = m_ClipFillType; - } else - { - e2FillType = m_ClipFillType; - e2FillType2 = m_SubjFillType; - } - - cInt e1Wc, e2Wc; - switch (e1FillType) - { - case pftPositive: e1Wc = e1->WindCnt; break; - case pftNegative: e1Wc = -e1->WindCnt; break; - default: e1Wc = Abs(e1->WindCnt); - } - switch(e2FillType) - { - case pftPositive: e2Wc = e2->WindCnt; break; - case pftNegative: e2Wc = -e2->WindCnt; break; - default: e2Wc = Abs(e2->WindCnt); - } - - if ( e1Contributing && e2Contributing ) - { - if ((e1Wc != 0 && e1Wc != 1) || (e2Wc != 0 && e2Wc != 1) || - (e1->PolyTyp != e2->PolyTyp && m_ClipType != ctXor) ) - { - AddLocalMaxPoly(e1, e2, Pt); - } - else - { - AddOutPt(e1, Pt); - AddOutPt(e2, Pt); - SwapSides( *e1 , *e2 ); - SwapPolyIndexes( *e1 , *e2 ); - } - } - else if ( e1Contributing ) - { - if (e2Wc == 0 || e2Wc == 1) - { - AddOutPt(e1, Pt); - SwapSides(*e1, *e2); - SwapPolyIndexes(*e1, *e2); - } - } - else if ( e2Contributing ) - { - if (e1Wc == 0 || e1Wc == 1) - { - AddOutPt(e2, Pt); - SwapSides(*e1, *e2); - SwapPolyIndexes(*e1, *e2); - } - } - else if ( (e1Wc == 0 || e1Wc == 1) && (e2Wc == 0 || e2Wc == 1)) - { - //neither edge is currently contributing ... - - cInt e1Wc2, e2Wc2; - switch (e1FillType2) - { - case pftPositive: e1Wc2 = e1->WindCnt2; break; - case pftNegative : e1Wc2 = -e1->WindCnt2; break; - default: e1Wc2 = Abs(e1->WindCnt2); - } - switch (e2FillType2) - { - case pftPositive: e2Wc2 = e2->WindCnt2; break; - case pftNegative: e2Wc2 = -e2->WindCnt2; break; - default: e2Wc2 = Abs(e2->WindCnt2); - } - - if (e1->PolyTyp != e2->PolyTyp) - { - AddLocalMinPoly(e1, e2, Pt); - } - else if (e1Wc == 1 && e2Wc == 1) - switch( m_ClipType ) { - case ctIntersection: - if (e1Wc2 > 0 && e2Wc2 > 0) - AddLocalMinPoly(e1, e2, Pt); - break; - case ctUnion: - if ( e1Wc2 <= 0 && e2Wc2 <= 0 ) - AddLocalMinPoly(e1, e2, Pt); - break; - case ctDifference: - if (((e1->PolyTyp == ptClip) && (e1Wc2 > 0) && (e2Wc2 > 0)) || - ((e1->PolyTyp == ptSubject) && (e1Wc2 <= 0) && (e2Wc2 <= 0))) - AddLocalMinPoly(e1, e2, Pt); - break; - case ctXor: - AddLocalMinPoly(e1, e2, Pt); - } - else - SwapSides( *e1, *e2 ); - } -} -//------------------------------------------------------------------------------ - -void Clipper::SetHoleState(TEdge *e, OutRec *outrec) -{ - TEdge *e2 = e->PrevInAEL; - TEdge *eTmp = 0; - while (e2) - { - if (e2->OutIdx >= 0 && e2->WindDelta != 0) - { - if (!eTmp) eTmp = e2; - else if (eTmp->OutIdx == e2->OutIdx) eTmp = 0; - } - e2 = e2->PrevInAEL; - } - if (!eTmp) - { - outrec->FirstLeft = 0; - outrec->IsHole = false; - } - else - { - outrec->FirstLeft = m_PolyOuts[eTmp->OutIdx]; - outrec->IsHole = !outrec->FirstLeft->IsHole; - } -} -//------------------------------------------------------------------------------ - -OutRec* GetLowermostRec(OutRec *outRec1, OutRec *outRec2) -{ - //work out which polygon fragment has the correct hole state ... - if (!outRec1->BottomPt) - outRec1->BottomPt = GetBottomPt(outRec1->Pts); - if (!outRec2->BottomPt) - outRec2->BottomPt = GetBottomPt(outRec2->Pts); - OutPt *OutPt1 = outRec1->BottomPt; - OutPt *OutPt2 = outRec2->BottomPt; - if (OutPt1->Pt.Y > OutPt2->Pt.Y) return outRec1; - else if (OutPt1->Pt.Y < OutPt2->Pt.Y) return outRec2; - else if (OutPt1->Pt.X < OutPt2->Pt.X) return outRec1; - else if (OutPt1->Pt.X > OutPt2->Pt.X) return outRec2; - else if (OutPt1->Next == OutPt1) return outRec2; - else if (OutPt2->Next == OutPt2) return outRec1; - else if (FirstIsBottomPt(OutPt1, OutPt2)) return outRec1; - else return outRec2; -} -//------------------------------------------------------------------------------ - -bool OutRec1RightOfOutRec2(OutRec* outRec1, OutRec* outRec2) -{ - do - { - outRec1 = outRec1->FirstLeft; - if (outRec1 == outRec2) return true; - } while (outRec1); - return false; -} -//------------------------------------------------------------------------------ - -OutRec* Clipper::GetOutRec(int Idx) -{ - OutRec* outrec = m_PolyOuts[Idx]; - while (outrec != m_PolyOuts[outrec->Idx]) - outrec = m_PolyOuts[outrec->Idx]; - return outrec; -} -//------------------------------------------------------------------------------ - -void Clipper::AppendPolygon(TEdge *e1, TEdge *e2) -{ - //get the start and ends of both output polygons ... - OutRec *outRec1 = m_PolyOuts[e1->OutIdx]; - OutRec *outRec2 = m_PolyOuts[e2->OutIdx]; - - OutRec *holeStateRec; - if (OutRec1RightOfOutRec2(outRec1, outRec2)) - holeStateRec = outRec2; - else if (OutRec1RightOfOutRec2(outRec2, outRec1)) - holeStateRec = outRec1; - else - holeStateRec = GetLowermostRec(outRec1, outRec2); - - //get the start and ends of both output polygons and - //join e2 poly onto e1 poly and delete pointers to e2 ... - - OutPt* p1_lft = outRec1->Pts; - OutPt* p1_rt = p1_lft->Prev; - OutPt* p2_lft = outRec2->Pts; - OutPt* p2_rt = p2_lft->Prev; - - //join e2 poly onto e1 poly and delete pointers to e2 ... - if( e1->Side == esLeft ) - { - if( e2->Side == esLeft ) - { - //z y x a b c - ReversePolyPtLinks(p2_lft); - p2_lft->Next = p1_lft; - p1_lft->Prev = p2_lft; - p1_rt->Next = p2_rt; - p2_rt->Prev = p1_rt; - outRec1->Pts = p2_rt; - } else - { - //x y z a b c - p2_rt->Next = p1_lft; - p1_lft->Prev = p2_rt; - p2_lft->Prev = p1_rt; - p1_rt->Next = p2_lft; - outRec1->Pts = p2_lft; - } - } else - { - if( e2->Side == esRight ) - { - //a b c z y x - ReversePolyPtLinks(p2_lft); - p1_rt->Next = p2_rt; - p2_rt->Prev = p1_rt; - p2_lft->Next = p1_lft; - p1_lft->Prev = p2_lft; - } else - { - //a b c x y z - p1_rt->Next = p2_lft; - p2_lft->Prev = p1_rt; - p1_lft->Prev = p2_rt; - p2_rt->Next = p1_lft; - } - } - - outRec1->BottomPt = 0; - if (holeStateRec == outRec2) - { - if (outRec2->FirstLeft != outRec1) - outRec1->FirstLeft = outRec2->FirstLeft; - outRec1->IsHole = outRec2->IsHole; - } - outRec2->Pts = 0; - outRec2->BottomPt = 0; - outRec2->FirstLeft = outRec1; - - int OKIdx = e1->OutIdx; - int ObsoleteIdx = e2->OutIdx; - - e1->OutIdx = Unassigned; //nb: safe because we only get here via AddLocalMaxPoly - e2->OutIdx = Unassigned; - - TEdge* e = m_ActiveEdges; - while( e ) - { - if( e->OutIdx == ObsoleteIdx ) - { - e->OutIdx = OKIdx; - e->Side = e1->Side; - break; - } - e = e->NextInAEL; - } - - outRec2->Idx = outRec1->Idx; -} -//------------------------------------------------------------------------------ - -OutPt* Clipper::AddOutPt(TEdge *e, const IntPoint &pt) -{ - if( e->OutIdx < 0 ) - { - OutRec *outRec = CreateOutRec(); - outRec->IsOpen = (e->WindDelta == 0); - OutPt* newOp = new OutPt; - outRec->Pts = newOp; - newOp->Idx = outRec->Idx; - newOp->Pt = pt; - newOp->Next = newOp; - newOp->Prev = newOp; - if (!outRec->IsOpen) - SetHoleState(e, outRec); - e->OutIdx = outRec->Idx; - return newOp; - } else - { - OutRec *outRec = m_PolyOuts[e->OutIdx]; - //OutRec.Pts is the 'Left-most' point & OutRec.Pts.Prev is the 'Right-most' - OutPt* op = outRec->Pts; - - bool ToFront = (e->Side == esLeft); - if (ToFront && (pt == op->Pt)) return op; - else if (!ToFront && (pt == op->Prev->Pt)) return op->Prev; - - OutPt* newOp = new OutPt; - newOp->Idx = outRec->Idx; - newOp->Pt = pt; - newOp->Next = op; - newOp->Prev = op->Prev; - newOp->Prev->Next = newOp; - op->Prev = newOp; - if (ToFront) outRec->Pts = newOp; - return newOp; - } -} -//------------------------------------------------------------------------------ - -OutPt* Clipper::GetLastOutPt(TEdge *e) -{ - OutRec *outRec = m_PolyOuts[e->OutIdx]; - if (e->Side == esLeft) - return outRec->Pts; - else - return outRec->Pts->Prev; -} -//------------------------------------------------------------------------------ - -void Clipper::ProcessHorizontals() -{ - TEdge* horzEdge; - while (PopEdgeFromSEL(horzEdge)) - ProcessHorizontal(horzEdge); -} -//------------------------------------------------------------------------------ - -inline bool IsMinima(TEdge *e) -{ - return e && (e->Prev->NextInLML != e) && (e->Next->NextInLML != e); -} -//------------------------------------------------------------------------------ - -inline bool IsMaxima(TEdge *e, const cInt Y) -{ - return e && e->Top.Y == Y && !e->NextInLML; -} -//------------------------------------------------------------------------------ - -inline bool IsIntermediate(TEdge *e, const cInt Y) -{ - return e->Top.Y == Y && e->NextInLML; -} -//------------------------------------------------------------------------------ - -TEdge *GetMaximaPair(TEdge *e) -{ - if ((e->Next->Top == e->Top) && !e->Next->NextInLML) - return e->Next; - else if ((e->Prev->Top == e->Top) && !e->Prev->NextInLML) - return e->Prev; - else return 0; -} -//------------------------------------------------------------------------------ - -TEdge *GetMaximaPairEx(TEdge *e) -{ - //as GetMaximaPair() but returns 0 if MaxPair isn't in AEL (unless it's horizontal) - TEdge* result = GetMaximaPair(e); - if (result && (result->OutIdx == Skip || - (result->NextInAEL == result->PrevInAEL && !IsHorizontal(*result)))) return 0; - return result; -} -//------------------------------------------------------------------------------ - -void Clipper::SwapPositionsInSEL(TEdge *Edge1, TEdge *Edge2) -{ - if( !( Edge1->NextInSEL ) && !( Edge1->PrevInSEL ) ) return; - if( !( Edge2->NextInSEL ) && !( Edge2->PrevInSEL ) ) return; - - if( Edge1->NextInSEL == Edge2 ) - { - TEdge* Next = Edge2->NextInSEL; - if( Next ) Next->PrevInSEL = Edge1; - TEdge* Prev = Edge1->PrevInSEL; - if( Prev ) Prev->NextInSEL = Edge2; - Edge2->PrevInSEL = Prev; - Edge2->NextInSEL = Edge1; - Edge1->PrevInSEL = Edge2; - Edge1->NextInSEL = Next; - } - else if( Edge2->NextInSEL == Edge1 ) - { - TEdge* Next = Edge1->NextInSEL; - if( Next ) Next->PrevInSEL = Edge2; - TEdge* Prev = Edge2->PrevInSEL; - if( Prev ) Prev->NextInSEL = Edge1; - Edge1->PrevInSEL = Prev; - Edge1->NextInSEL = Edge2; - Edge2->PrevInSEL = Edge1; - Edge2->NextInSEL = Next; - } - else - { - TEdge* Next = Edge1->NextInSEL; - TEdge* Prev = Edge1->PrevInSEL; - Edge1->NextInSEL = Edge2->NextInSEL; - if( Edge1->NextInSEL ) Edge1->NextInSEL->PrevInSEL = Edge1; - Edge1->PrevInSEL = Edge2->PrevInSEL; - if( Edge1->PrevInSEL ) Edge1->PrevInSEL->NextInSEL = Edge1; - Edge2->NextInSEL = Next; - if( Edge2->NextInSEL ) Edge2->NextInSEL->PrevInSEL = Edge2; - Edge2->PrevInSEL = Prev; - if( Edge2->PrevInSEL ) Edge2->PrevInSEL->NextInSEL = Edge2; - } - - if( !Edge1->PrevInSEL ) m_SortedEdges = Edge1; - else if( !Edge2->PrevInSEL ) m_SortedEdges = Edge2; -} -//------------------------------------------------------------------------------ - -TEdge* GetNextInAEL(TEdge *e, Direction dir) -{ - return dir == dLeftToRight ? e->NextInAEL : e->PrevInAEL; -} -//------------------------------------------------------------------------------ - -void GetHorzDirection(TEdge& HorzEdge, Direction& Dir, cInt& Left, cInt& Right) -{ - if (HorzEdge.Bot.X < HorzEdge.Top.X) - { - Left = HorzEdge.Bot.X; - Right = HorzEdge.Top.X; - Dir = dLeftToRight; - } else - { - Left = HorzEdge.Top.X; - Right = HorzEdge.Bot.X; - Dir = dRightToLeft; - } -} -//------------------------------------------------------------------------ - -/******************************************************************************* -* Notes: Horizontal edges (HEs) at scanline intersections (ie at the Top or * -* Bottom of a scanbeam) are processed as if layered. The order in which HEs * -* are processed doesn't matter. HEs intersect with other HE Bot.Xs only [#] * -* (or they could intersect with Top.Xs only, ie EITHER Bot.Xs OR Top.Xs), * -* and with other non-horizontal edges [*]. Once these intersections are * -* processed, intermediate HEs then 'promote' the Edge above (NextInLML) into * -* the AEL. These 'promoted' edges may in turn intersect [%] with other HEs. * -*******************************************************************************/ - -void Clipper::ProcessHorizontal(TEdge *horzEdge) -{ - Direction dir; - cInt horzLeft, horzRight; - bool IsOpen = (horzEdge->WindDelta == 0); - - GetHorzDirection(*horzEdge, dir, horzLeft, horzRight); - - TEdge* eLastHorz = horzEdge, *eMaxPair = 0; - while (eLastHorz->NextInLML && IsHorizontal(*eLastHorz->NextInLML)) - eLastHorz = eLastHorz->NextInLML; - if (!eLastHorz->NextInLML) - eMaxPair = GetMaximaPair(eLastHorz); - - MaximaList::const_iterator maxIt; - MaximaList::const_reverse_iterator maxRit; - if (m_Maxima.size() > 0) - { - //get the first maxima in range (X) ... - if (dir == dLeftToRight) - { - maxIt = m_Maxima.begin(); - while (maxIt != m_Maxima.end() && *maxIt <= horzEdge->Bot.X) maxIt++; - if (maxIt != m_Maxima.end() && *maxIt >= eLastHorz->Top.X) - maxIt = m_Maxima.end(); - } - else - { - maxRit = m_Maxima.rbegin(); - while (maxRit != m_Maxima.rend() && *maxRit > horzEdge->Bot.X) maxRit++; - if (maxRit != m_Maxima.rend() && *maxRit <= eLastHorz->Top.X) - maxRit = m_Maxima.rend(); - } - } - - OutPt* op1 = 0; - - for (;;) //loop through consec. horizontal edges - { - - bool IsLastHorz = (horzEdge == eLastHorz); - TEdge* e = GetNextInAEL(horzEdge, dir); - while(e) - { - - //this code block inserts extra coords into horizontal edges (in output - //polygons) whereever maxima touch these horizontal edges. This helps - //'simplifying' polygons (ie if the Simplify property is set). - if (m_Maxima.size() > 0) - { - if (dir == dLeftToRight) - { - while (maxIt != m_Maxima.end() && *maxIt < e->Curr.X) - { - if (horzEdge->OutIdx >= 0 && !IsOpen) - AddOutPt(horzEdge, IntPoint(*maxIt, horzEdge->Bot.Y)); - maxIt++; - } - } - else - { - while (maxRit != m_Maxima.rend() && *maxRit > e->Curr.X) - { - if (horzEdge->OutIdx >= 0 && !IsOpen) - AddOutPt(horzEdge, IntPoint(*maxRit, horzEdge->Bot.Y)); - maxRit++; - } - } - }; - - if ((dir == dLeftToRight && e->Curr.X > horzRight) || - (dir == dRightToLeft && e->Curr.X < horzLeft)) break; - - //Also break if we've got to the end of an intermediate horizontal edge ... - //nb: Smaller Dx's are to the right of larger Dx's ABOVE the horizontal. - if (e->Curr.X == horzEdge->Top.X && horzEdge->NextInLML && - e->Dx < horzEdge->NextInLML->Dx) break; - - if (horzEdge->OutIdx >= 0 && !IsOpen) //note: may be done multiple times - { -#ifdef use_xyz - if (dir == dLeftToRight) SetZ(e->Curr, *horzEdge, *e); - else SetZ(e->Curr, *e, *horzEdge); -#endif - op1 = AddOutPt(horzEdge, e->Curr); - TEdge* eNextHorz = m_SortedEdges; - while (eNextHorz) - { - if (eNextHorz->OutIdx >= 0 && - HorzSegmentsOverlap(horzEdge->Bot.X, - horzEdge->Top.X, eNextHorz->Bot.X, eNextHorz->Top.X)) - { - OutPt* op2 = GetLastOutPt(eNextHorz); - AddJoin(op2, op1, eNextHorz->Top); - } - eNextHorz = eNextHorz->NextInSEL; - } - AddGhostJoin(op1, horzEdge->Bot); - } - - //OK, so far we're still in range of the horizontal Edge but make sure - //we're at the last of consec. horizontals when matching with eMaxPair - if(e == eMaxPair && IsLastHorz) - { - if (horzEdge->OutIdx >= 0) - AddLocalMaxPoly(horzEdge, eMaxPair, horzEdge->Top); - DeleteFromAEL(horzEdge); - DeleteFromAEL(eMaxPair); - return; - } - - if(dir == dLeftToRight) - { - IntPoint Pt = IntPoint(e->Curr.X, horzEdge->Curr.Y); - IntersectEdges(horzEdge, e, Pt); - } - else - { - IntPoint Pt = IntPoint(e->Curr.X, horzEdge->Curr.Y); - IntersectEdges( e, horzEdge, Pt); - } - TEdge* eNext = GetNextInAEL(e, dir); - SwapPositionsInAEL( horzEdge, e ); - e = eNext; - } //end while(e) - - //Break out of loop if HorzEdge.NextInLML is not also horizontal ... - if (!horzEdge->NextInLML || !IsHorizontal(*horzEdge->NextInLML)) break; - - UpdateEdgeIntoAEL(horzEdge); - if (horzEdge->OutIdx >= 0) AddOutPt(horzEdge, horzEdge->Bot); - GetHorzDirection(*horzEdge, dir, horzLeft, horzRight); - - } //end for (;;) - - if (horzEdge->OutIdx >= 0 && !op1) - { - op1 = GetLastOutPt(horzEdge); - TEdge* eNextHorz = m_SortedEdges; - while (eNextHorz) - { - if (eNextHorz->OutIdx >= 0 && - HorzSegmentsOverlap(horzEdge->Bot.X, - horzEdge->Top.X, eNextHorz->Bot.X, eNextHorz->Top.X)) - { - OutPt* op2 = GetLastOutPt(eNextHorz); - AddJoin(op2, op1, eNextHorz->Top); - } - eNextHorz = eNextHorz->NextInSEL; - } - AddGhostJoin(op1, horzEdge->Top); - } - - if (horzEdge->NextInLML) - { - if(horzEdge->OutIdx >= 0) - { - op1 = AddOutPt( horzEdge, horzEdge->Top); - UpdateEdgeIntoAEL(horzEdge); - if (horzEdge->WindDelta == 0) return; - //nb: HorzEdge is no longer horizontal here - TEdge* ePrev = horzEdge->PrevInAEL; - TEdge* eNext = horzEdge->NextInAEL; - if (ePrev && ePrev->Curr.X == horzEdge->Bot.X && - ePrev->Curr.Y == horzEdge->Bot.Y && ePrev->WindDelta != 0 && - (ePrev->OutIdx >= 0 && ePrev->Curr.Y > ePrev->Top.Y && - SlopesEqual(*horzEdge, *ePrev, m_UseFullRange))) - { - OutPt* op2 = AddOutPt(ePrev, horzEdge->Bot); - AddJoin(op1, op2, horzEdge->Top); - } - else if (eNext && eNext->Curr.X == horzEdge->Bot.X && - eNext->Curr.Y == horzEdge->Bot.Y && eNext->WindDelta != 0 && - eNext->OutIdx >= 0 && eNext->Curr.Y > eNext->Top.Y && - SlopesEqual(*horzEdge, *eNext, m_UseFullRange)) - { - OutPt* op2 = AddOutPt(eNext, horzEdge->Bot); - AddJoin(op1, op2, horzEdge->Top); - } - } - else - UpdateEdgeIntoAEL(horzEdge); - } - else - { - if (horzEdge->OutIdx >= 0) AddOutPt(horzEdge, horzEdge->Top); - DeleteFromAEL(horzEdge); - } -} -//------------------------------------------------------------------------------ - -bool Clipper::ProcessIntersections(const cInt topY) -{ - if( !m_ActiveEdges ) return true; - try { - BuildIntersectList(topY); - size_t IlSize = m_IntersectList.size(); - if (IlSize == 0) return true; - if (IlSize == 1 || FixupIntersectionOrder()) ProcessIntersectList(); - else return false; - } - catch(...) - { - m_SortedEdges = 0; - DisposeIntersectNodes(); - throw clipperException("ProcessIntersections error"); - } - m_SortedEdges = 0; - return true; -} -//------------------------------------------------------------------------------ - -void Clipper::DisposeIntersectNodes() -{ - for (size_t i = 0; i < m_IntersectList.size(); ++i ) - delete m_IntersectList[i]; - m_IntersectList.clear(); -} -//------------------------------------------------------------------------------ - -void Clipper::BuildIntersectList(const cInt topY) -{ - if ( !m_ActiveEdges ) return; - - //prepare for sorting ... - TEdge* e = m_ActiveEdges; - m_SortedEdges = e; - while( e ) - { - e->PrevInSEL = e->PrevInAEL; - e->NextInSEL = e->NextInAEL; - e->Curr.X = TopX( *e, topY ); - e = e->NextInAEL; - } - - //bubblesort ... - bool isModified; - do - { - isModified = false; - e = m_SortedEdges; - while( e->NextInSEL ) - { - TEdge *eNext = e->NextInSEL; - IntPoint Pt; - if(e->Curr.X > eNext->Curr.X) - { - IntersectPoint(*e, *eNext, Pt); - if (Pt.Y < topY) Pt = IntPoint(TopX(*e, topY), topY); - IntersectNode * newNode = new IntersectNode; - newNode->Edge1 = e; - newNode->Edge2 = eNext; - newNode->Pt = Pt; - m_IntersectList.push_back(newNode); - - SwapPositionsInSEL(e, eNext); - isModified = true; - } - else - e = eNext; - } - if( e->PrevInSEL ) e->PrevInSEL->NextInSEL = 0; - else break; - } - while ( isModified ); - m_SortedEdges = 0; //important -} -//------------------------------------------------------------------------------ - - -void Clipper::ProcessIntersectList() -{ - for (size_t i = 0; i < m_IntersectList.size(); ++i) - { - IntersectNode* iNode = m_IntersectList[i]; - { - IntersectEdges( iNode->Edge1, iNode->Edge2, iNode->Pt); - SwapPositionsInAEL( iNode->Edge1 , iNode->Edge2 ); - } - delete iNode; - } - m_IntersectList.clear(); -} -//------------------------------------------------------------------------------ - -bool IntersectListSort(IntersectNode* node1, IntersectNode* node2) -{ - return node2->Pt.Y < node1->Pt.Y; -} -//------------------------------------------------------------------------------ - -inline bool EdgesAdjacent(const IntersectNode &inode) -{ - return (inode.Edge1->NextInSEL == inode.Edge2) || - (inode.Edge1->PrevInSEL == inode.Edge2); -} -//------------------------------------------------------------------------------ - -bool Clipper::FixupIntersectionOrder() -{ - //pre-condition: intersections are sorted Bottom-most first. - //Now it's crucial that intersections are made only between adjacent edges, - //so to ensure this the order of intersections may need adjusting ... - CopyAELToSEL(); - std::sort(m_IntersectList.begin(), m_IntersectList.end(), IntersectListSort); - size_t cnt = m_IntersectList.size(); - for (size_t i = 0; i < cnt; ++i) - { - if (!EdgesAdjacent(*m_IntersectList[i])) - { - size_t j = i + 1; - while (j < cnt && !EdgesAdjacent(*m_IntersectList[j])) j++; - if (j == cnt) return false; - std::swap(m_IntersectList[i], m_IntersectList[j]); - } - SwapPositionsInSEL(m_IntersectList[i]->Edge1, m_IntersectList[i]->Edge2); - } - return true; -} -//------------------------------------------------------------------------------ - -void Clipper::DoMaxima(TEdge *e) -{ - TEdge* eMaxPair = GetMaximaPairEx(e); - if (!eMaxPair) - { - if (e->OutIdx >= 0) - AddOutPt(e, e->Top); - DeleteFromAEL(e); - return; - } - - TEdge* eNext = e->NextInAEL; - while(eNext && eNext != eMaxPair) - { - IntersectEdges(e, eNext, e->Top); - SwapPositionsInAEL(e, eNext); - eNext = e->NextInAEL; - } - - if(e->OutIdx == Unassigned && eMaxPair->OutIdx == Unassigned) - { - DeleteFromAEL(e); - DeleteFromAEL(eMaxPair); - } - else if( e->OutIdx >= 0 && eMaxPair->OutIdx >= 0 ) - { - if (e->OutIdx >= 0) AddLocalMaxPoly(e, eMaxPair, e->Top); - DeleteFromAEL(e); - DeleteFromAEL(eMaxPair); - } -#ifdef use_lines - else if (e->WindDelta == 0) - { - if (e->OutIdx >= 0) - { - AddOutPt(e, e->Top); - e->OutIdx = Unassigned; - } - DeleteFromAEL(e); - - if (eMaxPair->OutIdx >= 0) - { - AddOutPt(eMaxPair, e->Top); - eMaxPair->OutIdx = Unassigned; - } - DeleteFromAEL(eMaxPair); - } -#endif - else throw clipperException("DoMaxima error"); -} -//------------------------------------------------------------------------------ - -void Clipper::ProcessEdgesAtTopOfScanbeam(const cInt topY) -{ - TEdge* e = m_ActiveEdges; - while( e ) - { - //1. process maxima, treating them as if they're 'bent' horizontal edges, - // but exclude maxima with horizontal edges. nb: e can't be a horizontal. - bool IsMaximaEdge = IsMaxima(e, topY); - - if(IsMaximaEdge) - { - TEdge* eMaxPair = GetMaximaPairEx(e); - IsMaximaEdge = (!eMaxPair || !IsHorizontal(*eMaxPair)); - } - - if(IsMaximaEdge) - { - if (m_StrictSimple) m_Maxima.push_back(e->Top.X); - TEdge* ePrev = e->PrevInAEL; - DoMaxima(e); - if( !ePrev ) e = m_ActiveEdges; - else e = ePrev->NextInAEL; - } - else - { - //2. promote horizontal edges, otherwise update Curr.X and Curr.Y ... - if (IsIntermediate(e, topY) && IsHorizontal(*e->NextInLML)) - { - UpdateEdgeIntoAEL(e); - if (e->OutIdx >= 0) - AddOutPt(e, e->Bot); - AddEdgeToSEL(e); - } - else - { - e->Curr.X = TopX( *e, topY ); - e->Curr.Y = topY; -#ifdef use_xyz - e->Curr.Z = topY == e->Top.Y ? e->Top.Z : (topY == e->Bot.Y ? e->Bot.Z : 0); -#endif - } - - //When StrictlySimple and 'e' is being touched by another edge, then - //make sure both edges have a vertex here ... - if (m_StrictSimple) - { - TEdge* ePrev = e->PrevInAEL; - if ((e->OutIdx >= 0) && (e->WindDelta != 0) && ePrev && (ePrev->OutIdx >= 0) && - (ePrev->Curr.X == e->Curr.X) && (ePrev->WindDelta != 0)) - { - IntPoint pt = e->Curr; -#ifdef use_xyz - SetZ(pt, *ePrev, *e); -#endif - OutPt* op = AddOutPt(ePrev, pt); - OutPt* op2 = AddOutPt(e, pt); - AddJoin(op, op2, pt); //StrictlySimple (type-3) join - } - } - - e = e->NextInAEL; - } - } - - //3. Process horizontals at the Top of the scanbeam ... - m_Maxima.sort(); - ProcessHorizontals(); - m_Maxima.clear(); - - //4. Promote intermediate vertices ... - e = m_ActiveEdges; - while(e) - { - if(IsIntermediate(e, topY)) - { - OutPt* op = 0; - if( e->OutIdx >= 0 ) - op = AddOutPt(e, e->Top); - UpdateEdgeIntoAEL(e); - - //if output polygons share an edge, they'll need joining later ... - TEdge* ePrev = e->PrevInAEL; - TEdge* eNext = e->NextInAEL; - if (ePrev && ePrev->Curr.X == e->Bot.X && - ePrev->Curr.Y == e->Bot.Y && op && - ePrev->OutIdx >= 0 && ePrev->Curr.Y > ePrev->Top.Y && - SlopesEqual(e->Curr, e->Top, ePrev->Curr, ePrev->Top, m_UseFullRange) && - (e->WindDelta != 0) && (ePrev->WindDelta != 0)) - { - OutPt* op2 = AddOutPt(ePrev, e->Bot); - AddJoin(op, op2, e->Top); - } - else if (eNext && eNext->Curr.X == e->Bot.X && - eNext->Curr.Y == e->Bot.Y && op && - eNext->OutIdx >= 0 && eNext->Curr.Y > eNext->Top.Y && - SlopesEqual(e->Curr, e->Top, eNext->Curr, eNext->Top, m_UseFullRange) && - (e->WindDelta != 0) && (eNext->WindDelta != 0)) - { - OutPt* op2 = AddOutPt(eNext, e->Bot); - AddJoin(op, op2, e->Top); - } - } - e = e->NextInAEL; - } -} -//------------------------------------------------------------------------------ - -void Clipper::FixupOutPolyline(OutRec &outrec) -{ - OutPt *pp = outrec.Pts; - OutPt *lastPP = pp->Prev; - while (pp != lastPP) - { - pp = pp->Next; - if (pp->Pt == pp->Prev->Pt) - { - if (pp == lastPP) lastPP = pp->Prev; - OutPt *tmpPP = pp->Prev; - tmpPP->Next = pp->Next; - pp->Next->Prev = tmpPP; - delete pp; - pp = tmpPP; - } - } - - if (pp == pp->Prev) - { - DisposeOutPts(pp); - outrec.Pts = 0; - return; - } -} -//------------------------------------------------------------------------------ - -void Clipper::FixupOutPolygon(OutRec &outrec) -{ - //FixupOutPolygon() - removes duplicate points and simplifies consecutive - //parallel edges by removing the middle vertex. - OutPt *lastOK = 0; - outrec.BottomPt = 0; - OutPt *pp = outrec.Pts; - bool preserveCol = m_PreserveCollinear || m_StrictSimple; - - for (;;) - { - if (pp->Prev == pp || pp->Prev == pp->Next) - { - DisposeOutPts(pp); - outrec.Pts = 0; - return; - } - - //test for duplicate points and collinear edges ... - if ((pp->Pt == pp->Next->Pt) || (pp->Pt == pp->Prev->Pt) || - (SlopesEqual(pp->Prev->Pt, pp->Pt, pp->Next->Pt, m_UseFullRange) && - (!preserveCol || !Pt2IsBetweenPt1AndPt3(pp->Prev->Pt, pp->Pt, pp->Next->Pt)))) - { - lastOK = 0; - OutPt *tmp = pp; - pp->Prev->Next = pp->Next; - pp->Next->Prev = pp->Prev; - pp = pp->Prev; - delete tmp; - } - else if (pp == lastOK) break; - else - { - if (!lastOK) lastOK = pp; - pp = pp->Next; - } - } - outrec.Pts = pp; -} -//------------------------------------------------------------------------------ - -int PointCount(OutPt *Pts) -{ - if (!Pts) return 0; - int result = 0; - OutPt* p = Pts; - do - { - result++; - p = p->Next; - } - while (p != Pts); - return result; -} -//------------------------------------------------------------------------------ - -void Clipper::BuildResult(Paths &polys) -{ - polys.reserve(m_PolyOuts.size()); - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i) - { - if (!m_PolyOuts[i]->Pts) continue; - Path pg; - OutPt* p = m_PolyOuts[i]->Pts->Prev; - int cnt = PointCount(p); - if (cnt < 2) continue; - pg.reserve(cnt); - for (int i = 0; i < cnt; ++i) - { - pg.push_back(p->Pt); - p = p->Prev; - } - polys.push_back(pg); - } -} -//------------------------------------------------------------------------------ - -void Clipper::BuildResult2(PolyTree& polytree) -{ - polytree.Clear(); - polytree.AllNodes.reserve(m_PolyOuts.size()); - //add each output polygon/contour to polytree ... - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); i++) - { - OutRec* outRec = m_PolyOuts[i]; - int cnt = PointCount(outRec->Pts); - if ((outRec->IsOpen && cnt < 2) || (!outRec->IsOpen && cnt < 3)) continue; - FixHoleLinkage(*outRec); - PolyNode* pn = new PolyNode(); - //nb: polytree takes ownership of all the PolyNodes - polytree.AllNodes.push_back(pn); - outRec->PolyNd = pn; - pn->Parent = 0; - pn->Index = 0; - pn->Contour.reserve(cnt); - OutPt *op = outRec->Pts->Prev; - for (int j = 0; j < cnt; j++) - { - pn->Contour.push_back(op->Pt); - op = op->Prev; - } - } - - //fixup PolyNode links etc ... - polytree.Childs.reserve(m_PolyOuts.size()); - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); i++) - { - OutRec* outRec = m_PolyOuts[i]; - if (!outRec->PolyNd) continue; - if (outRec->IsOpen) - { - outRec->PolyNd->m_IsOpen = true; - polytree.AddChild(*outRec->PolyNd); - } - else if (outRec->FirstLeft && outRec->FirstLeft->PolyNd) - outRec->FirstLeft->PolyNd->AddChild(*outRec->PolyNd); - else - polytree.AddChild(*outRec->PolyNd); - } -} -//------------------------------------------------------------------------------ - -void SwapIntersectNodes(IntersectNode &int1, IntersectNode &int2) -{ - //just swap the contents (because fIntersectNodes is a single-linked-list) - IntersectNode inode = int1; //gets a copy of Int1 - int1.Edge1 = int2.Edge1; - int1.Edge2 = int2.Edge2; - int1.Pt = int2.Pt; - int2.Edge1 = inode.Edge1; - int2.Edge2 = inode.Edge2; - int2.Pt = inode.Pt; -} -//------------------------------------------------------------------------------ - -inline bool E2InsertsBeforeE1(TEdge &e1, TEdge &e2) -{ - if (e2.Curr.X == e1.Curr.X) - { - if (e2.Top.Y > e1.Top.Y) - return e2.Top.X < TopX(e1, e2.Top.Y); - else return e1.Top.X > TopX(e2, e1.Top.Y); - } - else return e2.Curr.X < e1.Curr.X; -} -//------------------------------------------------------------------------------ - -bool GetOverlap(const cInt a1, const cInt a2, const cInt b1, const cInt b2, - cInt& Left, cInt& Right) -{ - if (a1 < a2) - { - if (b1 < b2) {Left = std::max(a1,b1); Right = std::min(a2,b2);} - else {Left = std::max(a1,b2); Right = std::min(a2,b1);} - } - else - { - if (b1 < b2) {Left = std::max(a2,b1); Right = std::min(a1,b2);} - else {Left = std::max(a2,b2); Right = std::min(a1,b1);} - } - return Left < Right; -} -//------------------------------------------------------------------------------ - -inline void UpdateOutPtIdxs(OutRec& outrec) -{ - OutPt* op = outrec.Pts; - do - { - op->Idx = outrec.Idx; - op = op->Prev; - } - while(op != outrec.Pts); -} -//------------------------------------------------------------------------------ - -void Clipper::InsertEdgeIntoAEL(TEdge *edge, TEdge* startEdge) -{ - if(!m_ActiveEdges) - { - edge->PrevInAEL = 0; - edge->NextInAEL = 0; - m_ActiveEdges = edge; - } - else if(!startEdge && E2InsertsBeforeE1(*m_ActiveEdges, *edge)) - { - edge->PrevInAEL = 0; - edge->NextInAEL = m_ActiveEdges; - m_ActiveEdges->PrevInAEL = edge; - m_ActiveEdges = edge; - } - else - { - if(!startEdge) startEdge = m_ActiveEdges; - while(startEdge->NextInAEL && - !E2InsertsBeforeE1(*startEdge->NextInAEL , *edge)) - startEdge = startEdge->NextInAEL; - edge->NextInAEL = startEdge->NextInAEL; - if(startEdge->NextInAEL) startEdge->NextInAEL->PrevInAEL = edge; - edge->PrevInAEL = startEdge; - startEdge->NextInAEL = edge; - } -} -//---------------------------------------------------------------------- - -OutPt* DupOutPt(OutPt* outPt, bool InsertAfter) -{ - OutPt* result = new OutPt; - result->Pt = outPt->Pt; - result->Idx = outPt->Idx; - if (InsertAfter) - { - result->Next = outPt->Next; - result->Prev = outPt; - outPt->Next->Prev = result; - outPt->Next = result; - } - else - { - result->Prev = outPt->Prev; - result->Next = outPt; - outPt->Prev->Next = result; - outPt->Prev = result; - } - return result; -} -//------------------------------------------------------------------------------ - -bool JoinHorz(OutPt* op1, OutPt* op1b, OutPt* op2, OutPt* op2b, - const IntPoint Pt, bool DiscardLeft) -{ - Direction Dir1 = (op1->Pt.X > op1b->Pt.X ? dRightToLeft : dLeftToRight); - Direction Dir2 = (op2->Pt.X > op2b->Pt.X ? dRightToLeft : dLeftToRight); - if (Dir1 == Dir2) return false; - - //When DiscardLeft, we want Op1b to be on the Left of Op1, otherwise we - //want Op1b to be on the Right. (And likewise with Op2 and Op2b.) - //So, to facilitate this while inserting Op1b and Op2b ... - //when DiscardLeft, make sure we're AT or RIGHT of Pt before adding Op1b, - //otherwise make sure we're AT or LEFT of Pt. (Likewise with Op2b.) - if (Dir1 == dLeftToRight) - { - while (op1->Next->Pt.X <= Pt.X && - op1->Next->Pt.X >= op1->Pt.X && op1->Next->Pt.Y == Pt.Y) - op1 = op1->Next; - if (DiscardLeft && (op1->Pt.X != Pt.X)) op1 = op1->Next; - op1b = DupOutPt(op1, !DiscardLeft); - if (op1b->Pt != Pt) - { - op1 = op1b; - op1->Pt = Pt; - op1b = DupOutPt(op1, !DiscardLeft); - } - } - else - { - while (op1->Next->Pt.X >= Pt.X && - op1->Next->Pt.X <= op1->Pt.X && op1->Next->Pt.Y == Pt.Y) - op1 = op1->Next; - if (!DiscardLeft && (op1->Pt.X != Pt.X)) op1 = op1->Next; - op1b = DupOutPt(op1, DiscardLeft); - if (op1b->Pt != Pt) - { - op1 = op1b; - op1->Pt = Pt; - op1b = DupOutPt(op1, DiscardLeft); - } - } - - if (Dir2 == dLeftToRight) - { - while (op2->Next->Pt.X <= Pt.X && - op2->Next->Pt.X >= op2->Pt.X && op2->Next->Pt.Y == Pt.Y) - op2 = op2->Next; - if (DiscardLeft && (op2->Pt.X != Pt.X)) op2 = op2->Next; - op2b = DupOutPt(op2, !DiscardLeft); - if (op2b->Pt != Pt) - { - op2 = op2b; - op2->Pt = Pt; - op2b = DupOutPt(op2, !DiscardLeft); - }; - } else - { - while (op2->Next->Pt.X >= Pt.X && - op2->Next->Pt.X <= op2->Pt.X && op2->Next->Pt.Y == Pt.Y) - op2 = op2->Next; - if (!DiscardLeft && (op2->Pt.X != Pt.X)) op2 = op2->Next; - op2b = DupOutPt(op2, DiscardLeft); - if (op2b->Pt != Pt) - { - op2 = op2b; - op2->Pt = Pt; - op2b = DupOutPt(op2, DiscardLeft); - }; - }; - - if ((Dir1 == dLeftToRight) == DiscardLeft) - { - op1->Prev = op2; - op2->Next = op1; - op1b->Next = op2b; - op2b->Prev = op1b; - } - else - { - op1->Next = op2; - op2->Prev = op1; - op1b->Prev = op2b; - op2b->Next = op1b; - } - return true; -} -//------------------------------------------------------------------------------ - -bool Clipper::JoinPoints(Join *j, OutRec* outRec1, OutRec* outRec2) -{ - OutPt *op1 = j->OutPt1, *op1b; - OutPt *op2 = j->OutPt2, *op2b; - - //There are 3 kinds of joins for output polygons ... - //1. Horizontal joins where Join.OutPt1 & Join.OutPt2 are vertices anywhere - //along (horizontal) collinear edges (& Join.OffPt is on the same horizontal). - //2. Non-horizontal joins where Join.OutPt1 & Join.OutPt2 are at the same - //location at the Bottom of the overlapping segment (& Join.OffPt is above). - //3. StrictSimple joins where edges touch but are not collinear and where - //Join.OutPt1, Join.OutPt2 & Join.OffPt all share the same point. - bool isHorizontal = (j->OutPt1->Pt.Y == j->OffPt.Y); - - if (isHorizontal && (j->OffPt == j->OutPt1->Pt) && - (j->OffPt == j->OutPt2->Pt)) - { - //Strictly Simple join ... - if (outRec1 != outRec2) return false; - op1b = j->OutPt1->Next; - while (op1b != op1 && (op1b->Pt == j->OffPt)) - op1b = op1b->Next; - bool reverse1 = (op1b->Pt.Y > j->OffPt.Y); - op2b = j->OutPt2->Next; - while (op2b != op2 && (op2b->Pt == j->OffPt)) - op2b = op2b->Next; - bool reverse2 = (op2b->Pt.Y > j->OffPt.Y); - if (reverse1 == reverse2) return false; - if (reverse1) - { - op1b = DupOutPt(op1, false); - op2b = DupOutPt(op2, true); - op1->Prev = op2; - op2->Next = op1; - op1b->Next = op2b; - op2b->Prev = op1b; - j->OutPt1 = op1; - j->OutPt2 = op1b; - return true; - } else - { - op1b = DupOutPt(op1, true); - op2b = DupOutPt(op2, false); - op1->Next = op2; - op2->Prev = op1; - op1b->Prev = op2b; - op2b->Next = op1b; - j->OutPt1 = op1; - j->OutPt2 = op1b; - return true; - } - } - else if (isHorizontal) - { - //treat horizontal joins differently to non-horizontal joins since with - //them we're not yet sure where the overlapping is. OutPt1.Pt & OutPt2.Pt - //may be anywhere along the horizontal edge. - op1b = op1; - while (op1->Prev->Pt.Y == op1->Pt.Y && op1->Prev != op1b && op1->Prev != op2) - op1 = op1->Prev; - while (op1b->Next->Pt.Y == op1b->Pt.Y && op1b->Next != op1 && op1b->Next != op2) - op1b = op1b->Next; - if (op1b->Next == op1 || op1b->Next == op2) return false; //a flat 'polygon' - - op2b = op2; - while (op2->Prev->Pt.Y == op2->Pt.Y && op2->Prev != op2b && op2->Prev != op1b) - op2 = op2->Prev; - while (op2b->Next->Pt.Y == op2b->Pt.Y && op2b->Next != op2 && op2b->Next != op1) - op2b = op2b->Next; - if (op2b->Next == op2 || op2b->Next == op1) return false; //a flat 'polygon' - - cInt Left, Right; - //Op1 --> Op1b & Op2 --> Op2b are the extremites of the horizontal edges - if (!GetOverlap(op1->Pt.X, op1b->Pt.X, op2->Pt.X, op2b->Pt.X, Left, Right)) - return false; - - //DiscardLeftSide: when overlapping edges are joined, a spike will created - //which needs to be cleaned up. However, we don't want Op1 or Op2 caught up - //on the discard Side as either may still be needed for other joins ... - IntPoint Pt; - bool DiscardLeftSide; - if (op1->Pt.X >= Left && op1->Pt.X <= Right) - { - Pt = op1->Pt; DiscardLeftSide = (op1->Pt.X > op1b->Pt.X); - } - else if (op2->Pt.X >= Left&& op2->Pt.X <= Right) - { - Pt = op2->Pt; DiscardLeftSide = (op2->Pt.X > op2b->Pt.X); - } - else if (op1b->Pt.X >= Left && op1b->Pt.X <= Right) - { - Pt = op1b->Pt; DiscardLeftSide = op1b->Pt.X > op1->Pt.X; - } - else - { - Pt = op2b->Pt; DiscardLeftSide = (op2b->Pt.X > op2->Pt.X); - } - j->OutPt1 = op1; j->OutPt2 = op2; - return JoinHorz(op1, op1b, op2, op2b, Pt, DiscardLeftSide); - } else - { - //nb: For non-horizontal joins ... - // 1. Jr.OutPt1.Pt.Y == Jr.OutPt2.Pt.Y - // 2. Jr.OutPt1.Pt > Jr.OffPt.Y - - //make sure the polygons are correctly oriented ... - op1b = op1->Next; - while ((op1b->Pt == op1->Pt) && (op1b != op1)) op1b = op1b->Next; - bool Reverse1 = ((op1b->Pt.Y > op1->Pt.Y) || - !SlopesEqual(op1->Pt, op1b->Pt, j->OffPt, m_UseFullRange)); - if (Reverse1) - { - op1b = op1->Prev; - while ((op1b->Pt == op1->Pt) && (op1b != op1)) op1b = op1b->Prev; - if ((op1b->Pt.Y > op1->Pt.Y) || - !SlopesEqual(op1->Pt, op1b->Pt, j->OffPt, m_UseFullRange)) return false; - }; - op2b = op2->Next; - while ((op2b->Pt == op2->Pt) && (op2b != op2))op2b = op2b->Next; - bool Reverse2 = ((op2b->Pt.Y > op2->Pt.Y) || - !SlopesEqual(op2->Pt, op2b->Pt, j->OffPt, m_UseFullRange)); - if (Reverse2) - { - op2b = op2->Prev; - while ((op2b->Pt == op2->Pt) && (op2b != op2)) op2b = op2b->Prev; - if ((op2b->Pt.Y > op2->Pt.Y) || - !SlopesEqual(op2->Pt, op2b->Pt, j->OffPt, m_UseFullRange)) return false; - } - - if ((op1b == op1) || (op2b == op2) || (op1b == op2b) || - ((outRec1 == outRec2) && (Reverse1 == Reverse2))) return false; - - if (Reverse1) - { - op1b = DupOutPt(op1, false); - op2b = DupOutPt(op2, true); - op1->Prev = op2; - op2->Next = op1; - op1b->Next = op2b; - op2b->Prev = op1b; - j->OutPt1 = op1; - j->OutPt2 = op1b; - return true; - } else - { - op1b = DupOutPt(op1, true); - op2b = DupOutPt(op2, false); - op1->Next = op2; - op2->Prev = op1; - op1b->Prev = op2b; - op2b->Next = op1b; - j->OutPt1 = op1; - j->OutPt2 = op1b; - return true; - } - } -} -//---------------------------------------------------------------------- - -static OutRec* ParseFirstLeft(OutRec* FirstLeft) -{ - while (FirstLeft && !FirstLeft->Pts) - FirstLeft = FirstLeft->FirstLeft; - return FirstLeft; -} -//------------------------------------------------------------------------------ - -void Clipper::FixupFirstLefts1(OutRec* OldOutRec, OutRec* NewOutRec) -{ - //tests if NewOutRec contains the polygon before reassigning FirstLeft - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i) - { - OutRec* outRec = m_PolyOuts[i]; - OutRec* firstLeft = ParseFirstLeft(outRec->FirstLeft); - if (outRec->Pts && firstLeft == OldOutRec) - { - if (Poly2ContainsPoly1(outRec->Pts, NewOutRec->Pts)) - outRec->FirstLeft = NewOutRec; - } - } -} -//---------------------------------------------------------------------- - -void Clipper::FixupFirstLefts2(OutRec* InnerOutRec, OutRec* OuterOutRec) -{ - //A polygon has split into two such that one is now the inner of the other. - //It's possible that these polygons now wrap around other polygons, so check - //every polygon that's also contained by OuterOutRec's FirstLeft container - //(including 0) to see if they've become inner to the new inner polygon ... - OutRec* orfl = OuterOutRec->FirstLeft; - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i) - { - OutRec* outRec = m_PolyOuts[i]; - - if (!outRec->Pts || outRec == OuterOutRec || outRec == InnerOutRec) - continue; - OutRec* firstLeft = ParseFirstLeft(outRec->FirstLeft); - if (firstLeft != orfl && firstLeft != InnerOutRec && firstLeft != OuterOutRec) - continue; - if (Poly2ContainsPoly1(outRec->Pts, InnerOutRec->Pts)) - outRec->FirstLeft = InnerOutRec; - else if (Poly2ContainsPoly1(outRec->Pts, OuterOutRec->Pts)) - outRec->FirstLeft = OuterOutRec; - else if (outRec->FirstLeft == InnerOutRec || outRec->FirstLeft == OuterOutRec) - outRec->FirstLeft = orfl; - } -} -//---------------------------------------------------------------------- -void Clipper::FixupFirstLefts3(OutRec* OldOutRec, OutRec* NewOutRec) -{ - //reassigns FirstLeft WITHOUT testing if NewOutRec contains the polygon - for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i) - { - OutRec* outRec = m_PolyOuts[i]; - OutRec* firstLeft = ParseFirstLeft(outRec->FirstLeft); - if (outRec->Pts && firstLeft == OldOutRec) - outRec->FirstLeft = NewOutRec; - } -} -//---------------------------------------------------------------------- - -void Clipper::JoinCommonEdges() -{ - for (JoinList::size_type i = 0; i < m_Joins.size(); i++) - { - Join* join = m_Joins[i]; - - OutRec *outRec1 = GetOutRec(join->OutPt1->Idx); - OutRec *outRec2 = GetOutRec(join->OutPt2->Idx); - - if (!outRec1->Pts || !outRec2->Pts) continue; - if (outRec1->IsOpen || outRec2->IsOpen) continue; - - //get the polygon fragment with the correct hole state (FirstLeft) - //before calling JoinPoints() ... - OutRec *holeStateRec; - if (outRec1 == outRec2) holeStateRec = outRec1; - else if (OutRec1RightOfOutRec2(outRec1, outRec2)) holeStateRec = outRec2; - else if (OutRec1RightOfOutRec2(outRec2, outRec1)) holeStateRec = outRec1; - else holeStateRec = GetLowermostRec(outRec1, outRec2); - - if (!JoinPoints(join, outRec1, outRec2)) continue; - - if (outRec1 == outRec2) - { - //instead of joining two polygons, we've just created a new one by - //splitting one polygon into two. - outRec1->Pts = join->OutPt1; - outRec1->BottomPt = 0; - outRec2 = CreateOutRec(); - outRec2->Pts = join->OutPt2; - - //update all OutRec2.Pts Idx's ... - UpdateOutPtIdxs(*outRec2); - - if (Poly2ContainsPoly1(outRec2->Pts, outRec1->Pts)) - { - //outRec1 contains outRec2 ... - outRec2->IsHole = !outRec1->IsHole; - outRec2->FirstLeft = outRec1; - - if (m_UsingPolyTree) FixupFirstLefts2(outRec2, outRec1); - - if ((outRec2->IsHole ^ m_ReverseOutput) == (Area(*outRec2) > 0)) - ReversePolyPtLinks(outRec2->Pts); - - } else if (Poly2ContainsPoly1(outRec1->Pts, outRec2->Pts)) - { - //outRec2 contains outRec1 ... - outRec2->IsHole = outRec1->IsHole; - outRec1->IsHole = !outRec2->IsHole; - outRec2->FirstLeft = outRec1->FirstLeft; - outRec1->FirstLeft = outRec2; - - if (m_UsingPolyTree) FixupFirstLefts2(outRec1, outRec2); - - if ((outRec1->IsHole ^ m_ReverseOutput) == (Area(*outRec1) > 0)) - ReversePolyPtLinks(outRec1->Pts); - } - else - { - //the 2 polygons are completely separate ... - outRec2->IsHole = outRec1->IsHole; - outRec2->FirstLeft = outRec1->FirstLeft; - - //fixup FirstLeft pointers that may need reassigning to OutRec2 - if (m_UsingPolyTree) FixupFirstLefts1(outRec1, outRec2); - } - - } else - { - //joined 2 polygons together ... - - outRec2->Pts = 0; - outRec2->BottomPt = 0; - outRec2->Idx = outRec1->Idx; - - outRec1->IsHole = holeStateRec->IsHole; - if (holeStateRec == outRec2) - outRec1->FirstLeft = outRec2->FirstLeft; - outRec2->FirstLeft = outRec1; - - if (m_UsingPolyTree) FixupFirstLefts3(outRec2, outRec1); - } - } -} - -//------------------------------------------------------------------------------ -// ClipperOffset support functions ... -//------------------------------------------------------------------------------ - -DoublePoint GetUnitNormal(const IntPoint &pt1, const IntPoint &pt2) -{ - if(pt2.X == pt1.X && pt2.Y == pt1.Y) - return DoublePoint(0, 0); - - double Dx = (double)(pt2.X - pt1.X); - double dy = (double)(pt2.Y - pt1.Y); - double f = 1 *1.0/ std::sqrt( Dx*Dx + dy*dy ); - Dx *= f; - dy *= f; - return DoublePoint(dy, -Dx); -} - -//------------------------------------------------------------------------------ -// ClipperOffset class -//------------------------------------------------------------------------------ - -ClipperOffset::ClipperOffset(double miterLimit, double arcTolerance) -{ - this->MiterLimit = miterLimit; - this->ArcTolerance = arcTolerance; - m_lowest.X = -1; -} -//------------------------------------------------------------------------------ - -ClipperOffset::~ClipperOffset() -{ - Clear(); -} -//------------------------------------------------------------------------------ - -void ClipperOffset::Clear() -{ - for (int i = 0; i < m_polyNodes.ChildCount(); ++i) - delete m_polyNodes.Childs[i]; - m_polyNodes.Childs.clear(); - m_lowest.X = -1; -} -//------------------------------------------------------------------------------ - -void ClipperOffset::AddPath(const Path& path, JoinType joinType, EndType endType) -{ - int highI = (int)path.size() - 1; - if (highI < 0) return; - PolyNode* newNode = new PolyNode(); - newNode->m_jointype = joinType; - newNode->m_endtype = endType; - - //strip duplicate points from path and also get index to the lowest point ... - if (endType == etClosedLine || endType == etClosedPolygon) - while (highI > 0 && path[0] == path[highI]) highI--; - newNode->Contour.reserve(highI + 1); - newNode->Contour.push_back(path[0]); - int j = 0, k = 0; - for (int i = 1; i <= highI; i++) - if (newNode->Contour[j] != path[i]) - { - j++; - newNode->Contour.push_back(path[i]); - if (path[i].Y > newNode->Contour[k].Y || - (path[i].Y == newNode->Contour[k].Y && - path[i].X < newNode->Contour[k].X)) k = j; - } - if (endType == etClosedPolygon && j < 2) - { - delete newNode; - return; - } - m_polyNodes.AddChild(*newNode); - - //if this path's lowest pt is lower than all the others then update m_lowest - if (endType != etClosedPolygon) return; - if (m_lowest.X < 0) - m_lowest = IntPoint(m_polyNodes.ChildCount() - 1, k); - else - { - IntPoint ip = m_polyNodes.Childs[(int)m_lowest.X]->Contour[(int)m_lowest.Y]; - if (newNode->Contour[k].Y > ip.Y || - (newNode->Contour[k].Y == ip.Y && - newNode->Contour[k].X < ip.X)) - m_lowest = IntPoint(m_polyNodes.ChildCount() - 1, k); - } -} -//------------------------------------------------------------------------------ - -void ClipperOffset::AddPaths(const Paths& paths, JoinType joinType, EndType endType) -{ - for (Paths::size_type i = 0; i < paths.size(); ++i) - AddPath(paths[i], joinType, endType); -} -//------------------------------------------------------------------------------ - -void ClipperOffset::FixOrientations() -{ - //fixup orientations of all closed paths if the orientation of the - //closed path with the lowermost vertex is wrong ... - if (m_lowest.X >= 0 && - !Orientation(m_polyNodes.Childs[(int)m_lowest.X]->Contour)) - { - for (int i = 0; i < m_polyNodes.ChildCount(); ++i) - { - PolyNode& node = *m_polyNodes.Childs[i]; - if (node.m_endtype == etClosedPolygon || - (node.m_endtype == etClosedLine && Orientation(node.Contour))) - ReversePath(node.Contour); - } - } else - { - for (int i = 0; i < m_polyNodes.ChildCount(); ++i) - { - PolyNode& node = *m_polyNodes.Childs[i]; - if (node.m_endtype == etClosedLine && !Orientation(node.Contour)) - ReversePath(node.Contour); - } - } -} -//------------------------------------------------------------------------------ - -void ClipperOffset::Execute(Paths& solution, double delta) -{ - solution.clear(); - FixOrientations(); - DoOffset(delta); - - //now clean up 'corners' ... - Clipper clpr; - clpr.AddPaths(m_destPolys, ptSubject, true); - if (delta > 0) - { - clpr.Execute(ctUnion, solution, pftPositive, pftPositive); - } - else - { - IntRect r = clpr.GetBounds(); - Path outer(4); - outer[0] = IntPoint(r.left - 10, r.bottom + 10); - outer[1] = IntPoint(r.right + 10, r.bottom + 10); - outer[2] = IntPoint(r.right + 10, r.top - 10); - outer[3] = IntPoint(r.left - 10, r.top - 10); - - clpr.AddPath(outer, ptSubject, true); - clpr.ReverseSolution(true); - clpr.Execute(ctUnion, solution, pftNegative, pftNegative); - if (solution.size() > 0) solution.erase(solution.begin()); - } -} -//------------------------------------------------------------------------------ - -void ClipperOffset::Execute(PolyTree& solution, double delta) -{ - solution.Clear(); - FixOrientations(); - DoOffset(delta); - - //now clean up 'corners' ... - Clipper clpr; - clpr.AddPaths(m_destPolys, ptSubject, true); - if (delta > 0) - { - clpr.Execute(ctUnion, solution, pftPositive, pftPositive); - } - else - { - IntRect r = clpr.GetBounds(); - Path outer(4); - outer[0] = IntPoint(r.left - 10, r.bottom + 10); - outer[1] = IntPoint(r.right + 10, r.bottom + 10); - outer[2] = IntPoint(r.right + 10, r.top - 10); - outer[3] = IntPoint(r.left - 10, r.top - 10); - - clpr.AddPath(outer, ptSubject, true); - clpr.ReverseSolution(true); - clpr.Execute(ctUnion, solution, pftNegative, pftNegative); - //remove the outer PolyNode rectangle ... - if (solution.ChildCount() == 1 && solution.Childs[0]->ChildCount() > 0) - { - PolyNode* outerNode = solution.Childs[0]; - solution.Childs.reserve(outerNode->ChildCount()); - solution.Childs[0] = outerNode->Childs[0]; - solution.Childs[0]->Parent = outerNode->Parent; - for (int i = 1; i < outerNode->ChildCount(); ++i) - solution.AddChild(*outerNode->Childs[i]); - } - else - solution.Clear(); - } -} -//------------------------------------------------------------------------------ - -void ClipperOffset::DoOffset(double delta) -{ - m_destPolys.clear(); - m_delta = delta; - - //if Zero offset, just copy any CLOSED polygons to m_p and return ... - if (NEAR_ZERO(delta)) - { - m_destPolys.reserve(m_polyNodes.ChildCount()); - for (int i = 0; i < m_polyNodes.ChildCount(); i++) - { - PolyNode& node = *m_polyNodes.Childs[i]; - if (node.m_endtype == etClosedPolygon) - m_destPolys.push_back(node.Contour); - } - return; - } - - //see offset_triginometry3.svg in the documentation folder ... - if (MiterLimit > 2) m_miterLim = 2/(MiterLimit * MiterLimit); - else m_miterLim = 0.5; - - double y; - if (ArcTolerance <= 0.0) y = def_arc_tolerance; - else if (ArcTolerance > std::fabs(delta) * def_arc_tolerance) - y = std::fabs(delta) * def_arc_tolerance; - else y = ArcTolerance; - //see offset_triginometry2.svg in the documentation folder ... - double steps = pi / std::acos(1 - y / std::fabs(delta)); - if (steps > std::fabs(delta) * pi) - steps = std::fabs(delta) * pi; //ie excessive precision check - m_sin = std::sin(two_pi / steps); - m_cos = std::cos(two_pi / steps); - m_StepsPerRad = steps / two_pi; - if (delta < 0.0) m_sin = -m_sin; - - m_destPolys.reserve(m_polyNodes.ChildCount() * 2); - for (int i = 0; i < m_polyNodes.ChildCount(); i++) - { - PolyNode& node = *m_polyNodes.Childs[i]; - m_srcPoly = node.Contour; - - int len = (int)m_srcPoly.size(); - if (len == 0 || (delta <= 0 && (len < 3 || node.m_endtype != etClosedPolygon))) - continue; - - m_destPoly.clear(); - if (len == 1) - { - if (node.m_jointype == jtRound) - { - double X = 1.0, Y = 0.0; - for (cInt j = 1; j <= steps; j++) - { - m_destPoly.push_back(IntPoint( - Round(m_srcPoly[0].X + X * delta), - Round(m_srcPoly[0].Y + Y * delta))); - double X2 = X; - X = X * m_cos - m_sin * Y; - Y = X2 * m_sin + Y * m_cos; - } - } - else - { - double X = -1.0, Y = -1.0; - for (int j = 0; j < 4; ++j) - { - m_destPoly.push_back(IntPoint( - Round(m_srcPoly[0].X + X * delta), - Round(m_srcPoly[0].Y + Y * delta))); - if (X < 0) X = 1; - else if (Y < 0) Y = 1; - else X = -1; - } - } - m_destPolys.push_back(m_destPoly); - continue; - } - //build m_normals ... - m_normals.clear(); - m_normals.reserve(len); - for (int j = 0; j < len - 1; ++j) - m_normals.push_back(GetUnitNormal(m_srcPoly[j], m_srcPoly[j + 1])); - if (node.m_endtype == etClosedLine || node.m_endtype == etClosedPolygon) - m_normals.push_back(GetUnitNormal(m_srcPoly[len - 1], m_srcPoly[0])); - else - m_normals.push_back(DoublePoint(m_normals[len - 2])); - - if (node.m_endtype == etClosedPolygon) - { - int k = len - 1; - for (int j = 0; j < len; ++j) - OffsetPoint(j, k, node.m_jointype); - m_destPolys.push_back(m_destPoly); - } - else if (node.m_endtype == etClosedLine) - { - int k = len - 1; - for (int j = 0; j < len; ++j) - OffsetPoint(j, k, node.m_jointype); - m_destPolys.push_back(m_destPoly); - m_destPoly.clear(); - //re-build m_normals ... - DoublePoint n = m_normals[len -1]; - for (int j = len - 1; j > 0; j--) - m_normals[j] = DoublePoint(-m_normals[j - 1].X, -m_normals[j - 1].Y); - m_normals[0] = DoublePoint(-n.X, -n.Y); - k = 0; - for (int j = len - 1; j >= 0; j--) - OffsetPoint(j, k, node.m_jointype); - m_destPolys.push_back(m_destPoly); - } - else - { - int k = 0; - for (int j = 1; j < len - 1; ++j) - OffsetPoint(j, k, node.m_jointype); - - IntPoint pt1; - if (node.m_endtype == etOpenButt) - { - int j = len - 1; - pt1 = IntPoint((cInt)Round(m_srcPoly[j].X + m_normals[j].X * - delta), (cInt)Round(m_srcPoly[j].Y + m_normals[j].Y * delta)); - m_destPoly.push_back(pt1); - pt1 = IntPoint((cInt)Round(m_srcPoly[j].X - m_normals[j].X * - delta), (cInt)Round(m_srcPoly[j].Y - m_normals[j].Y * delta)); - m_destPoly.push_back(pt1); - } - else - { - int j = len - 1; - k = len - 2; - m_sinA = 0; - m_normals[j] = DoublePoint(-m_normals[j].X, -m_normals[j].Y); - if (node.m_endtype == etOpenSquare) - DoSquare(j, k); - else - DoRound(j, k); - } - - //re-build m_normals ... - for (int j = len - 1; j > 0; j--) - m_normals[j] = DoublePoint(-m_normals[j - 1].X, -m_normals[j - 1].Y); - m_normals[0] = DoublePoint(-m_normals[1].X, -m_normals[1].Y); - - k = len - 1; - for (int j = k - 1; j > 0; --j) OffsetPoint(j, k, node.m_jointype); - - if (node.m_endtype == etOpenButt) - { - pt1 = IntPoint((cInt)Round(m_srcPoly[0].X - m_normals[0].X * delta), - (cInt)Round(m_srcPoly[0].Y - m_normals[0].Y * delta)); - m_destPoly.push_back(pt1); - pt1 = IntPoint((cInt)Round(m_srcPoly[0].X + m_normals[0].X * delta), - (cInt)Round(m_srcPoly[0].Y + m_normals[0].Y * delta)); - m_destPoly.push_back(pt1); - } - else - { - k = 1; - m_sinA = 0; - if (node.m_endtype == etOpenSquare) - DoSquare(0, 1); - else - DoRound(0, 1); - } - m_destPolys.push_back(m_destPoly); - } - } -} -//------------------------------------------------------------------------------ - -void ClipperOffset::OffsetPoint(int j, int& k, JoinType jointype) -{ - //cross product ... - m_sinA = (m_normals[k].X * m_normals[j].Y - m_normals[j].X * m_normals[k].Y); - if (std::fabs(m_sinA * m_delta) < 1.0) - { - //dot product ... - double cosA = (m_normals[k].X * m_normals[j].X + m_normals[j].Y * m_normals[k].Y ); - if (cosA > 0) // angle => 0 degrees - { - m_destPoly.push_back(IntPoint(Round(m_srcPoly[j].X + m_normals[k].X * m_delta), - Round(m_srcPoly[j].Y + m_normals[k].Y * m_delta))); - return; - } - //else angle => 180 degrees - } - else if (m_sinA > 1.0) m_sinA = 1.0; - else if (m_sinA < -1.0) m_sinA = -1.0; - - if (m_sinA * m_delta < 0) - { - m_destPoly.push_back(IntPoint(Round(m_srcPoly[j].X + m_normals[k].X * m_delta), - Round(m_srcPoly[j].Y + m_normals[k].Y * m_delta))); - m_destPoly.push_back(m_srcPoly[j]); - m_destPoly.push_back(IntPoint(Round(m_srcPoly[j].X + m_normals[j].X * m_delta), - Round(m_srcPoly[j].Y + m_normals[j].Y * m_delta))); - } - else - switch (jointype) - { - case jtMiter: - { - double r = 1 + (m_normals[j].X * m_normals[k].X + - m_normals[j].Y * m_normals[k].Y); - if (r >= m_miterLim) DoMiter(j, k, r); else DoSquare(j, k); - break; - } - case jtSquare: DoSquare(j, k); break; - case jtRound: DoRound(j, k); break; - } - k = j; -} -//------------------------------------------------------------------------------ - -void ClipperOffset::DoSquare(int j, int k) -{ - double dx = std::tan(std::atan2(m_sinA, - m_normals[k].X * m_normals[j].X + m_normals[k].Y * m_normals[j].Y) / 4); - m_destPoly.push_back(IntPoint( - Round(m_srcPoly[j].X + m_delta * (m_normals[k].X - m_normals[k].Y * dx)), - Round(m_srcPoly[j].Y + m_delta * (m_normals[k].Y + m_normals[k].X * dx)))); - m_destPoly.push_back(IntPoint( - Round(m_srcPoly[j].X + m_delta * (m_normals[j].X + m_normals[j].Y * dx)), - Round(m_srcPoly[j].Y + m_delta * (m_normals[j].Y - m_normals[j].X * dx)))); -} -//------------------------------------------------------------------------------ - -void ClipperOffset::DoMiter(int j, int k, double r) -{ - double q = m_delta / r; - m_destPoly.push_back(IntPoint(Round(m_srcPoly[j].X + (m_normals[k].X + m_normals[j].X) * q), - Round(m_srcPoly[j].Y + (m_normals[k].Y + m_normals[j].Y) * q))); -} -//------------------------------------------------------------------------------ - -void ClipperOffset::DoRound(int j, int k) -{ - double a = std::atan2(m_sinA, - m_normals[k].X * m_normals[j].X + m_normals[k].Y * m_normals[j].Y); - int steps = std::max((int)Round(m_StepsPerRad * std::fabs(a)), 1); - - double X = m_normals[k].X, Y = m_normals[k].Y, X2; - for (int i = 0; i < steps; ++i) - { - m_destPoly.push_back(IntPoint( - Round(m_srcPoly[j].X + X * m_delta), - Round(m_srcPoly[j].Y + Y * m_delta))); - X2 = X; - X = X * m_cos - m_sin * Y; - Y = X2 * m_sin + Y * m_cos; - } - m_destPoly.push_back(IntPoint( - Round(m_srcPoly[j].X + m_normals[j].X * m_delta), - Round(m_srcPoly[j].Y + m_normals[j].Y * m_delta))); -} - -//------------------------------------------------------------------------------ -// Miscellaneous public functions -//------------------------------------------------------------------------------ - -void Clipper::DoSimplePolygons() -{ - PolyOutList::size_type i = 0; - while (i < m_PolyOuts.size()) - { - OutRec* outrec = m_PolyOuts[i++]; - OutPt* op = outrec->Pts; - if (!op || outrec->IsOpen) continue; - do //for each Pt in Polygon until duplicate found do ... - { - OutPt* op2 = op->Next; - while (op2 != outrec->Pts) - { - if ((op->Pt == op2->Pt) && op2->Next != op && op2->Prev != op) - { - //split the polygon into two ... - OutPt* op3 = op->Prev; - OutPt* op4 = op2->Prev; - op->Prev = op4; - op4->Next = op; - op2->Prev = op3; - op3->Next = op2; - - outrec->Pts = op; - OutRec* outrec2 = CreateOutRec(); - outrec2->Pts = op2; - UpdateOutPtIdxs(*outrec2); - if (Poly2ContainsPoly1(outrec2->Pts, outrec->Pts)) - { - //OutRec2 is contained by OutRec1 ... - outrec2->IsHole = !outrec->IsHole; - outrec2->FirstLeft = outrec; - if (m_UsingPolyTree) FixupFirstLefts2(outrec2, outrec); - } - else - if (Poly2ContainsPoly1(outrec->Pts, outrec2->Pts)) - { - //OutRec1 is contained by OutRec2 ... - outrec2->IsHole = outrec->IsHole; - outrec->IsHole = !outrec2->IsHole; - outrec2->FirstLeft = outrec->FirstLeft; - outrec->FirstLeft = outrec2; - if (m_UsingPolyTree) FixupFirstLefts2(outrec, outrec2); - } - else - { - //the 2 polygons are separate ... - outrec2->IsHole = outrec->IsHole; - outrec2->FirstLeft = outrec->FirstLeft; - if (m_UsingPolyTree) FixupFirstLefts1(outrec, outrec2); - } - op2 = op; //ie get ready for the Next iteration - } - op2 = op2->Next; - } - op = op->Next; - } - while (op != outrec->Pts); - } -} -//------------------------------------------------------------------------------ - -void ReversePath(Path& p) -{ - std::reverse(p.begin(), p.end()); -} -//------------------------------------------------------------------------------ - -void ReversePaths(Paths& p) -{ - for (Paths::size_type i = 0; i < p.size(); ++i) - ReversePath(p[i]); -} -//------------------------------------------------------------------------------ - -void SimplifyPolygon(const Path &in_poly, Paths &out_polys, PolyFillType fillType) -{ - Clipper c; - c.StrictlySimple(true); - c.AddPath(in_poly, ptSubject, true); - c.Execute(ctUnion, out_polys, fillType, fillType); -} -//------------------------------------------------------------------------------ - -void SimplifyPolygons(const Paths &in_polys, Paths &out_polys, PolyFillType fillType) -{ - Clipper c; - c.StrictlySimple(true); - c.AddPaths(in_polys, ptSubject, true); - c.Execute(ctUnion, out_polys, fillType, fillType); -} -//------------------------------------------------------------------------------ - -void SimplifyPolygons(Paths &polys, PolyFillType fillType) -{ - SimplifyPolygons(polys, polys, fillType); -} -//------------------------------------------------------------------------------ - -inline double DistanceSqrd(const IntPoint& pt1, const IntPoint& pt2) -{ - double Dx = ((double)pt1.X - pt2.X); - double dy = ((double)pt1.Y - pt2.Y); - return (Dx*Dx + dy*dy); -} -//------------------------------------------------------------------------------ - -double DistanceFromLineSqrd( - const IntPoint& pt, const IntPoint& ln1, const IntPoint& ln2) -{ - //The equation of a line in general form (Ax + By + C = 0) - //given 2 points (x�,y�) & (x�,y�) is ... - //(y� - y�)x + (x� - x�)y + (y� - y�)x� - (x� - x�)y� = 0 - //A = (y� - y�); B = (x� - x�); C = (y� - y�)x� - (x� - x�)y� - //perpendicular distance of point (x�,y�) = (Ax� + By� + C)/Sqrt(A� + B�) - //see http://en.wikipedia.org/wiki/Perpendicular_distance - double A = double(ln1.Y - ln2.Y); - double B = double(ln2.X - ln1.X); - double C = A * ln1.X + B * ln1.Y; - C = A * pt.X + B * pt.Y - C; - return (C * C) / (A * A + B * B); -} -//--------------------------------------------------------------------------- - -bool SlopesNearCollinear(const IntPoint& pt1, - const IntPoint& pt2, const IntPoint& pt3, double distSqrd) -{ - //this function is more accurate when the point that's geometrically - //between the other 2 points is the one that's tested for distance. - //ie makes it more likely to pick up 'spikes' ... - if (Abs(pt1.X - pt2.X) > Abs(pt1.Y - pt2.Y)) - { - if ((pt1.X > pt2.X) == (pt1.X < pt3.X)) - return DistanceFromLineSqrd(pt1, pt2, pt3) < distSqrd; - else if ((pt2.X > pt1.X) == (pt2.X < pt3.X)) - return DistanceFromLineSqrd(pt2, pt1, pt3) < distSqrd; - else - return DistanceFromLineSqrd(pt3, pt1, pt2) < distSqrd; - } - else - { - if ((pt1.Y > pt2.Y) == (pt1.Y < pt3.Y)) - return DistanceFromLineSqrd(pt1, pt2, pt3) < distSqrd; - else if ((pt2.Y > pt1.Y) == (pt2.Y < pt3.Y)) - return DistanceFromLineSqrd(pt2, pt1, pt3) < distSqrd; - else - return DistanceFromLineSqrd(pt3, pt1, pt2) < distSqrd; - } -} -//------------------------------------------------------------------------------ - -bool PointsAreClose(IntPoint pt1, IntPoint pt2, double distSqrd) -{ - double Dx = (double)pt1.X - pt2.X; - double dy = (double)pt1.Y - pt2.Y; - return ((Dx * Dx) + (dy * dy) <= distSqrd); -} -//------------------------------------------------------------------------------ - -OutPt* ExcludeOp(OutPt* op) -{ - OutPt* result = op->Prev; - result->Next = op->Next; - op->Next->Prev = result; - result->Idx = 0; - return result; -} -//------------------------------------------------------------------------------ - -void CleanPolygon(const Path& in_poly, Path& out_poly, double distance) -{ - //distance = proximity in units/pixels below which vertices - //will be stripped. Default ~= sqrt(2). - - size_t size = in_poly.size(); - - if (size == 0) - { - out_poly.clear(); - return; - } - - OutPt* outPts = new OutPt[size]; - for (size_t i = 0; i < size; ++i) - { - outPts[i].Pt = in_poly[i]; - outPts[i].Next = &outPts[(i + 1) % size]; - outPts[i].Next->Prev = &outPts[i]; - outPts[i].Idx = 0; - } - - double distSqrd = distance * distance; - OutPt* op = &outPts[0]; - while (op->Idx == 0 && op->Next != op->Prev) - { - if (PointsAreClose(op->Pt, op->Prev->Pt, distSqrd)) - { - op = ExcludeOp(op); - size--; - } - else if (PointsAreClose(op->Prev->Pt, op->Next->Pt, distSqrd)) - { - ExcludeOp(op->Next); - op = ExcludeOp(op); - size -= 2; - } - else if (SlopesNearCollinear(op->Prev->Pt, op->Pt, op->Next->Pt, distSqrd)) - { - op = ExcludeOp(op); - size--; - } - else - { - op->Idx = 1; - op = op->Next; - } - } - - if (size < 3) size = 0; - out_poly.resize(size); - for (size_t i = 0; i < size; ++i) - { - out_poly[i] = op->Pt; - op = op->Next; - } - delete [] outPts; -} -//------------------------------------------------------------------------------ - -void CleanPolygon(Path& poly, double distance) -{ - CleanPolygon(poly, poly, distance); -} -//------------------------------------------------------------------------------ - -void CleanPolygons(const Paths& in_polys, Paths& out_polys, double distance) -{ - out_polys.resize(in_polys.size()); - for (Paths::size_type i = 0; i < in_polys.size(); ++i) - CleanPolygon(in_polys[i], out_polys[i], distance); -} -//------------------------------------------------------------------------------ - -void CleanPolygons(Paths& polys, double distance) -{ - CleanPolygons(polys, polys, distance); -} -//------------------------------------------------------------------------------ - -void Minkowski(const Path& poly, const Path& path, - Paths& solution, bool isSum, bool isClosed) -{ - int delta = (isClosed ? 1 : 0); - size_t polyCnt = poly.size(); - size_t pathCnt = path.size(); - Paths pp; - pp.reserve(pathCnt); - if (isSum) - for (size_t i = 0; i < pathCnt; ++i) - { - Path p; - p.reserve(polyCnt); - for (size_t j = 0; j < poly.size(); ++j) - p.push_back(IntPoint(path[i].X + poly[j].X, path[i].Y + poly[j].Y)); - pp.push_back(p); - } - else - for (size_t i = 0; i < pathCnt; ++i) - { - Path p; - p.reserve(polyCnt); - for (size_t j = 0; j < poly.size(); ++j) - p.push_back(IntPoint(path[i].X - poly[j].X, path[i].Y - poly[j].Y)); - pp.push_back(p); - } - - solution.clear(); - solution.reserve((pathCnt + delta) * (polyCnt + 1)); - for (size_t i = 0; i < pathCnt - 1 + delta; ++i) - for (size_t j = 0; j < polyCnt; ++j) - { - Path quad; - quad.reserve(4); - quad.push_back(pp[i % pathCnt][j % polyCnt]); - quad.push_back(pp[(i + 1) % pathCnt][j % polyCnt]); - quad.push_back(pp[(i + 1) % pathCnt][(j + 1) % polyCnt]); - quad.push_back(pp[i % pathCnt][(j + 1) % polyCnt]); - if (!Orientation(quad)) ReversePath(quad); - solution.push_back(quad); - } -} -//------------------------------------------------------------------------------ - -void MinkowskiSum(const Path& pattern, const Path& path, Paths& solution, bool pathIsClosed) -{ - Minkowski(pattern, path, solution, true, pathIsClosed); - Clipper c; - c.AddPaths(solution, ptSubject, true); - c.Execute(ctUnion, solution, pftNonZero, pftNonZero); -} -//------------------------------------------------------------------------------ - -void TranslatePath(const Path& input, Path& output, const IntPoint delta) -{ - //precondition: input != output - output.resize(input.size()); - for (size_t i = 0; i < input.size(); ++i) - output[i] = IntPoint(input[i].X + delta.X, input[i].Y + delta.Y); -} -//------------------------------------------------------------------------------ - -void MinkowskiSum(const Path& pattern, const Paths& paths, Paths& solution, bool pathIsClosed) -{ - Clipper c; - for (size_t i = 0; i < paths.size(); ++i) - { - Paths tmp; - Minkowski(pattern, paths[i], tmp, true, pathIsClosed); - c.AddPaths(tmp, ptSubject, true); - if (pathIsClosed) - { - Path tmp2; - TranslatePath(paths[i], tmp2, pattern[0]); - c.AddPath(tmp2, ptClip, true); - } - } - c.Execute(ctUnion, solution, pftNonZero, pftNonZero); -} -//------------------------------------------------------------------------------ - -void MinkowskiDiff(const Path& poly1, const Path& poly2, Paths& solution) -{ - Minkowski(poly1, poly2, solution, false, true); - Clipper c; - c.AddPaths(solution, ptSubject, true); - c.Execute(ctUnion, solution, pftNonZero, pftNonZero); -} -//------------------------------------------------------------------------------ - -enum NodeType {ntAny, ntOpen, ntClosed}; - -void AddPolyNodeToPaths(const PolyNode& polynode, NodeType nodetype, Paths& paths) -{ - bool match = true; - if (nodetype == ntClosed) match = !polynode.IsOpen(); - else if (nodetype == ntOpen) return; - - if (!polynode.Contour.empty() && match) - paths.push_back(polynode.Contour); - for (int i = 0; i < polynode.ChildCount(); ++i) - AddPolyNodeToPaths(*polynode.Childs[i], nodetype, paths); -} -//------------------------------------------------------------------------------ - -void PolyTreeToPaths(const PolyTree& polytree, Paths& paths) -{ - paths.resize(0); - paths.reserve(polytree.Total()); - AddPolyNodeToPaths(polytree, ntAny, paths); -} -//------------------------------------------------------------------------------ - -void ClosedPathsFromPolyTree(const PolyTree& polytree, Paths& paths) -{ - paths.resize(0); - paths.reserve(polytree.Total()); - AddPolyNodeToPaths(polytree, ntClosed, paths); -} -//------------------------------------------------------------------------------ - -void OpenPathsFromPolyTree(PolyTree& polytree, Paths& paths) -{ - paths.resize(0); - paths.reserve(polytree.Total()); - //Open paths are top level only, so ... - for (int i = 0; i < polytree.ChildCount(); ++i) - if (polytree.Childs[i]->IsOpen()) - paths.push_back(polytree.Childs[i]->Contour); -} -//------------------------------------------------------------------------------ - -std::ostream& operator <<(std::ostream &s, const IntPoint &p) -{ - s << "(" << p.X << "," << p.Y << ")"; - return s; -} -//------------------------------------------------------------------------------ - -std::ostream& operator <<(std::ostream &s, const Path &p) -{ - if (p.empty()) return s; - Path::size_type last = p.size() -1; - for (Path::size_type i = 0; i < last; i++) - s << "(" << p[i].X << "," << p[i].Y << "), "; - s << "(" << p[last].X << "," << p[last].Y << ")\n"; - return s; -} -//------------------------------------------------------------------------------ - -std::ostream& operator <<(std::ostream &s, const Paths &p) -{ - for (Paths::size_type i = 0; i < p.size(); i++) - s << p[i]; - s << "\n"; - return s; -} -//------------------------------------------------------------------------------ - -} //ClipperLib namespace diff --git a/src/Wima/Snake/clipper/clipper.hpp b/src/Wima/Snake/clipper/clipper.hpp deleted file mode 100644 index 33043fdb647578aef71adc99ba16091d3a182555..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/clipper/clipper.hpp +++ /dev/null @@ -1,406 +0,0 @@ -/******************************************************************************* -* * -* 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 -#include -#include -#include -#include -#include -#include -#include -#include - -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 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 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 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 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 - - diff --git a/src/Wima/Snake/json/CircularGenerator.SettingsGroup.json b/src/Wima/Snake/json/CircularGenerator.SettingsGroup.json deleted file mode 100644 index 9c63ddb042ecbc565969e26ca45f8af94e3545aa..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/json/CircularGenerator.SettingsGroup.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "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 -} -] -} diff --git a/src/Wima/Snake/json/LinearGenerator.SettingsGroup.json b/src/Wima/Snake/json/LinearGenerator.SettingsGroup.json deleted file mode 100644 index a24f3e1ba7807d90540b9bc18535052f54bf381e..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/json/LinearGenerator.SettingsGroup.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "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 -} -] -} diff --git a/src/Wima/Snake/mapbox/feature.hpp b/src/Wima/Snake/mapbox/feature.hpp deleted file mode 100644 index 8d03f4674c3fb71f15e436561f06e454e71c4b46..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/feature.hpp +++ /dev/null @@ -1,113 +0,0 @@ -#pragma once - -#include - -#include - -#include -#include -#include -#include - -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>, - mapbox::util::recursive_wrapper>>; - -struct value : value_base -{ - using value_base::value_base; -}; - -using property_map = std::unordered_map; - -// The same considerations and requirement for numeric types apply as for `value_base`. -using identifier = mapbox::util::variant; - -template -struct feature -{ - using coordinate_type = T; - using geometry_type = mapbox::geometry::geometry; // 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 -constexpr bool operator==(feature const& lhs, feature const& rhs) -{ - return lhs.id == rhs.id && lhs.geometry == rhs.geometry && lhs.properties == rhs.properties; -} - -template -constexpr bool operator!=(feature const& lhs, feature const& rhs) -{ - return !(lhs == rhs); -} - -template class Cont = std::vector> -struct feature_collection : Cont> -{ - using coordinate_type = T; - using feature_type = feature; - using container_type = Cont; - using size_type = typename container_type::size_type; - - template - feature_collection(Args&&... args) : container_type(std::forward(args)...) - { - } - feature_collection(std::initializer_list args) - : container_type(std::move(args)) {} -}; - -} // namespace feature -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry.hpp b/src/Wima/Snake/mapbox/geometry.hpp deleted file mode 100644 index 9caecd9a108c6b452f279c6649519977dd787ac1..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include diff --git a/src/Wima/Snake/mapbox/geometry/box.hpp b/src/Wima/Snake/mapbox/geometry/box.hpp deleted file mode 100644 index bb3ea5701423601db2a0c4d37b11b7378589a0ea..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/box.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include - -namespace mapbox { -namespace geometry { - -template -struct box -{ - using coordinate_type = T; - using point_type = point; - - constexpr box(point_type const& min_, point_type const& max_) - : min(min_), max(max_) - { - } - - point_type min; - point_type max; -}; - -template -constexpr bool operator==(box const& lhs, box const& rhs) -{ - return lhs.min == rhs.min && lhs.max == rhs.max; -} - -template -constexpr bool operator!=(box const& lhs, box const& rhs) -{ - return lhs.min != rhs.min || lhs.max != rhs.max; -} - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/empty.hpp b/src/Wima/Snake/mapbox/geometry/empty.hpp deleted file mode 100644 index 0ea446d8bc76dcca73f57563c41426fde6ba5c34..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/empty.hpp +++ /dev/null @@ -1,18 +0,0 @@ -#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 diff --git a/src/Wima/Snake/mapbox/geometry/envelope.hpp b/src/Wima/Snake/mapbox/geometry/envelope.hpp deleted file mode 100644 index e1f722fc540973322731bf73b80145a6950b5ebd..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/envelope.hpp +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace mapbox { -namespace geometry { - -template -box envelope(G const& geometry) -{ - using limits = std::numeric_limits; - - T min_t = limits::has_infinity ? -limits::infinity() : limits::min(); - T max_t = limits::has_infinity ? limits::infinity() : limits::max(); - - point min(max_t, max_t); - point max(min_t, min_t); - - for_each_point(geometry, [&](point 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(min, max); -} - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/for_each_point.hpp b/src/Wima/Snake/mapbox/geometry/for_each_point.hpp deleted file mode 100644 index d31b484ffd708c868b51dd1bc45f9e7d73b91158..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/for_each_point.hpp +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include - -namespace mapbox { -namespace geometry { - -template -void for_each_point(mapbox::geometry::empty const&, F&&) -{ -} - -template -auto for_each_point(Point&& point, F&& f) - -> decltype(point.x, point.y, void()) -{ - f(std::forward(point)); -} - -template -auto for_each_point(Container&& container, F&& f) - -> decltype(container.begin(), container.end(), void()); - -template -void for_each_point(mapbox::util::variant const& geom, F&& f) -{ - mapbox::util::variant::visit(geom, [&](auto const& g) { - for_each_point(g, f); - }); -} - -template -void for_each_point(mapbox::util::variant& geom, F&& f) -{ - mapbox::util::variant::visit(geom, [&](auto& g) { - for_each_point(g, f); - }); -} - -template -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 diff --git a/src/Wima/Snake/mapbox/geometry/geometry.hpp b/src/Wima/Snake/mapbox/geometry/geometry.hpp deleted file mode 100644 index fa1e7c31c4cf3cc2443d586a5422d3758ab4d63a..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/geometry.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include - -// stl -#include - -namespace mapbox { -namespace geometry { - -template class Cont = std::vector> -struct geometry_collection; - -template class Cont = std::vector> -using geometry_base = mapbox::util::variant, - line_string, - polygon, - multi_point, - multi_line_string, - multi_polygon, - geometry_collection>; - -template class Cont = std::vector> -struct geometry : geometry_base -{ - using coordinate_type = T; - using geometry_base::geometry_base; -}; - -template class Cont> -struct geometry_collection : Cont> -{ - using coordinate_type = T; - using geometry_type = geometry; - using container_type = Cont; - using size_type = typename container_type::size_type; - - template - geometry_collection(Args&&... args) : container_type(std::forward(args)...) - { - } - geometry_collection(std::initializer_list args) - : container_type(std::move(args)) {} -}; - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/line_string.hpp b/src/Wima/Snake/mapbox/geometry/line_string.hpp deleted file mode 100644 index a015f71d1cf96c73e4f69dbcf5ea115c3f6fc50f..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/line_string.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -// mapbox -#include -// stl -#include - -namespace mapbox { -namespace geometry { - -template class Cont = std::vector> -struct line_string : Cont> -{ - using coordinate_type = T; - using point_type = point; - using container_type = Cont; - using size_type = typename container_type::size_type; - - template - line_string(Args&&... args) : container_type(std::forward(args)...) - { - } - line_string(std::initializer_list args) - : container_type(std::move(args)) {} -}; - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/multi_line_string.hpp b/src/Wima/Snake/mapbox/geometry/multi_line_string.hpp deleted file mode 100644 index b528fa61be3bd963546b0f43d4c4f0f8f70668e0..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/multi_line_string.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -// mapbox -#include -// stl -#include - -namespace mapbox { -namespace geometry { - -template class Cont = std::vector> -struct multi_line_string : Cont> -{ - using coordinate_type = T; - using line_string_type = line_string; - using container_type = Cont; - using size_type = typename container_type::size_type; - - template - multi_line_string(Args&&... args) : container_type(std::forward(args)...) - { - } - multi_line_string(std::initializer_list args) - : container_type(std::move(args)) {} -}; - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/multi_point.hpp b/src/Wima/Snake/mapbox/geometry/multi_point.hpp deleted file mode 100644 index 060927bf2308f94b8dcd55442867769f92b90937..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/multi_point.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -// mapbox -#include -// stl -#include - -namespace mapbox { -namespace geometry { - -template class Cont = std::vector> -struct multi_point : Cont> -{ - using coordinate_type = T; - using point_type = point; - using container_type = Cont; - using size_type = typename container_type::size_type; - - template - multi_point(Args&&... args) : container_type(std::forward(args)...) - { - } - multi_point(std::initializer_list args) - : container_type(std::move(args)) {} -}; - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/multi_polygon.hpp b/src/Wima/Snake/mapbox/geometry/multi_polygon.hpp deleted file mode 100644 index 9546860ede4a368d5e2922a3825e80e1110130dd..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/multi_polygon.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -// mapbox -#include -// stl -#include - -namespace mapbox { -namespace geometry { - -template class Cont = std::vector> -struct multi_polygon : Cont> -{ - using coordinate_type = T; - using polygon_type = polygon; - using container_type = Cont; - using size_type = typename container_type::size_type; - - template - multi_polygon(Args&&... args) : container_type(std::forward(args)...) - { - } - multi_polygon(std::initializer_list args) - : container_type(std::move(args)) {} -}; - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/point.hpp b/src/Wima/Snake/mapbox/geometry/point.hpp deleted file mode 100644 index da8d67733f2a51708d5516d8fc4d52a63bacd3ef..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/point.hpp +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -namespace mapbox { -namespace geometry { - -template -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 -constexpr bool operator==(point const& lhs, point const& rhs) -{ - return lhs.x == rhs.x && lhs.y == rhs.y; -} - -#pragma GCC diagnostic pop - -template -constexpr bool operator!=(point const& lhs, point const& rhs) -{ - return !(lhs == rhs); -} - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp b/src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp deleted file mode 100644 index 0c4c63278a58c07f605cdcda7134e87201e931aa..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp +++ /dev/null @@ -1,119 +0,0 @@ -#pragma once - -namespace mapbox { -namespace geometry { - -template -point operator+(point const& lhs, point const& rhs) -{ - return point(lhs.x + rhs.x, lhs.y + rhs.y); -} - -template -point operator+(point const& lhs, T const& rhs) -{ - return point(lhs.x + rhs, lhs.y + rhs); -} - -template -point operator-(point const& lhs, point const& rhs) -{ - return point(lhs.x - rhs.x, lhs.y - rhs.y); -} - -template -point operator-(point const& lhs, T const& rhs) -{ - return point(lhs.x - rhs, lhs.y - rhs); -} - -template -point operator*(point const& lhs, point const& rhs) -{ - return point(lhs.x * rhs.x, lhs.y * rhs.y); -} - -template -point operator*(point const& lhs, T const& rhs) -{ - return point(lhs.x * rhs, lhs.y * rhs); -} - -template -point operator/(point const& lhs, point const& rhs) -{ - return point(lhs.x / rhs.x, lhs.y / rhs.y); -} - -template -point operator/(point const& lhs, T const& rhs) -{ - return point(lhs.x / rhs, lhs.y / rhs); -} - -template -point& operator+=(point& lhs, point const& rhs) -{ - lhs.x += rhs.x; - lhs.y += rhs.y; - return lhs; -} - -template -point& operator+=(point& lhs, T const& rhs) -{ - lhs.x += rhs; - lhs.y += rhs; - return lhs; -} - -template -point& operator-=(point& lhs, point const& rhs) -{ - lhs.x -= rhs.x; - lhs.y -= rhs.y; - return lhs; -} - -template -point& operator-=(point& lhs, T const& rhs) -{ - lhs.x -= rhs; - lhs.y -= rhs; - return lhs; -} - -template -point& operator*=(point& lhs, point const& rhs) -{ - lhs.x *= rhs.x; - lhs.y *= rhs.y; - return lhs; -} - -template -point& operator*=(point& lhs, T const& rhs) -{ - lhs.x *= rhs; - lhs.y *= rhs; - return lhs; -} - -template -point& operator/=(point& lhs, point const& rhs) -{ - lhs.x /= rhs.x; - lhs.y /= rhs.y; - return lhs; -} - -template -point& operator/=(point& lhs, T const& rhs) -{ - lhs.x /= rhs; - lhs.y /= rhs; - return lhs; -} - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry/polygon.hpp b/src/Wima/Snake/mapbox/geometry/polygon.hpp deleted file mode 100644 index 5cad7bbf5580e476c1d2c2d9f524f24e68bf66a4..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry/polygon.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -// mapbox -#include - -// stl -#include - -namespace mapbox { -namespace geometry { - -template class Cont = std::vector> -struct linear_ring : Cont> -{ - using coordinate_type = T; - using point_type = point; - using container_type = Cont; - using size_type = typename container_type::size_type; - - template - linear_ring(Args&&... args) : container_type(std::forward(args)...) - { - } - linear_ring(std::initializer_list args) - : container_type(std::move(args)) {} -}; - -template class Cont = std::vector> -struct polygon : Cont> -{ - using coordinate_type = T; - using linear_ring_type = linear_ring; - using container_type = Cont; - using size_type = typename container_type::size_type; - - template - polygon(Args&&... args) : container_type(std::forward(args)...) - { - } - polygon(std::initializer_list args) - : container_type(std::move(args)) {} -}; - -} // namespace geometry -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/geometry_io.hpp b/src/Wima/Snake/mapbox/geometry_io.hpp deleted file mode 100644 index 4d91ccc961a79cfd303cac3b5c52230a448a9933..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/geometry_io.hpp +++ /dev/null @@ -1,98 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - -namespace mapbox { -namespace geometry { - -std::ostream& operator<<(std::ostream& os, const empty&) -{ - return os << "[]"; -} - -template -std::ostream& operator<<(std::ostream& os, const point& point) -{ - return os << "[" << point.x << "," << point.y << "]"; -} - -template class C, class... Args> -std::ostream& operator<<(std::ostream& os, const C& cont) -{ - os << "["; - for (auto it = cont.cbegin();;) - { - os << *it; - if (++it == cont.cend()) - { - break; - } - os << ","; - } - return os << "]"; -} - -template -std::ostream& operator<<(std::ostream& os, const line_string& geom) -{ - return os << static_cast::container_type>(geom); -} - -template -std::ostream& operator<<(std::ostream& os, const linear_ring& geom) -{ - return os << static_cast::container_type>(geom); -} - -template -std::ostream& operator<<(std::ostream& os, const polygon& geom) -{ - return os << static_cast::container_type>(geom); -} - -template -std::ostream& operator<<(std::ostream& os, const multi_point& geom) -{ - return os << static_cast::container_type>(geom); -} - -template -std::ostream& operator<<(std::ostream& os, const multi_line_string& geom) -{ - return os << static_cast::container_type>(geom); -} - -template -std::ostream& operator<<(std::ostream& os, const multi_polygon& geom) -{ - return os << static_cast::container_type>(geom); -} - -template -std::ostream& operator<<(std::ostream& os, const geometry& geom) -{ - geometry::visit(geom, [&](const auto& g) { os << g; }); - return os; -} - -template -std::ostream& operator<<(std::ostream& os, const geometry_collection& geom) -{ - return os << static_cast::container_type>(geom); -} - -} // namespace geometry - -namespace feature { - -std::ostream& operator<<(std::ostream& os, const null_value_t&) -{ - return os << "[]"; -} - -} // namespace feature -} // namespace mapbox diff --git a/src/Wima/Snake/mapbox/optional.hpp b/src/Wima/Snake/mapbox/optional.hpp deleted file mode 100644 index 1185894e7363497e7529a0eb8abb51e6d09e38a7..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/optional.hpp +++ /dev/null @@ -1,74 +0,0 @@ -#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 -#include - -#include "variant.hpp" - -namespace mapbox { -namespace util { - -template -class optional -{ - static_assert(!std::is_reference::value, "optional doesn't support references"); - - struct none_type - { - }; - - variant 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 const& get() const { return variant_.template get(); } - T& get() { return variant_.template get(); } - - 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 - void emplace(Args&&... args) - { - variant_ = T{std::forward(args)...}; - } - - void reset() { variant_ = none_type{}; } - -}; // class optional - -} // namespace util -} // namespace mapbox - -#endif // MAPBOX_UTIL_OPTIONAL_HPP diff --git a/src/Wima/Snake/mapbox/polylabel.hpp b/src/Wima/Snake/mapbox/polylabel.hpp deleted file mode 100644 index 0e9e2b5433a311c72a6182125890c0f09ca39742..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/polylabel.hpp +++ /dev/null @@ -1,178 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace mapbox { - -namespace detail { - -// get squared distance from a point to a segment -template -T getSegDistSq(const geometry::point& p, - const geometry::point& a, - const geometry::point& 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 -auto pointToPolygonDist(const geometry::point& point, const geometry::polygon& polygon) { - bool inside = false; - auto minDistSq = std::numeric_limits::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 -struct Cell { - Cell(const geometry::point& c_, T h_, const geometry::polygon& polygon) - : c(c_), - h(h_), - d(pointToPolygonDist(c, polygon)), - max(d + h * std::sqrt(2)) - {} - - geometry::point 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 -Cell getCentroidCell(const geometry::polygon& polygon) { - T area = 0; - geometry::point 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& a = ring[i]; - const geometry::point& 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(area == 0 ? ring.at(0) : c / area, 0, polygon); -} - -} // namespace detail - -template -geometry::point polylabel(const geometry::polygon& polygon, T precision = 1, bool debug = false) { - using namespace detail; - - // find the bounding box of the outer ring - const geometry::box envelope = geometry::envelope(polygon.at(0)); - - const geometry::point 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& a, const Cell& b) { - return a.max < b.max; - }; - using Queue = std::priority_queue, std::vector>, 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({x + h, y + h}, h, polygon)); - } - } - - // take centroid as the first best guess - auto bestCell = getCentroidCell(polygon); - - // special case for rectangular polygons - Cell 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({cell.c.x - h, cell.c.y - h}, h, polygon)); - cellQueue.push(Cell({cell.c.x + h, cell.c.y - h}, h, polygon)); - cellQueue.push(Cell({cell.c.x - h, cell.c.y + h}, h, polygon)); - cellQueue.push(Cell({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 diff --git a/src/Wima/Snake/mapbox/recursive_wrapper.hpp b/src/Wima/Snake/mapbox/recursive_wrapper.hpp deleted file mode 100644 index 4becdd689d46649987c7779717ff4965752698b1..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/recursive_wrapper.hpp +++ /dev/null @@ -1,122 +0,0 @@ -#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 -#include - -namespace mapbox { -namespace util { - -template -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 -inline void swap(recursive_wrapper& lhs, recursive_wrapper& rhs) noexcept -{ - lhs.swap(rhs); -} -} // namespace util -} // namespace mapbox - -#endif // MAPBOX_UTIL_RECURSIVE_WRAPPER_HPP diff --git a/src/Wima/Snake/mapbox/variant.hpp b/src/Wima/Snake/mapbox/variant.hpp deleted file mode 100644 index db5d3c86b58caf3a787e9b8e8d3fc7d8e5042fa9..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/mapbox/variant.hpp +++ /dev/null @@ -1,901 +0,0 @@ -#ifndef MAPBOX_UTIL_VARIANT_HPP -#define MAPBOX_UTIL_VARIANT_HPP - -#include -#include // size_t -#include // operator new -#include // runtime_error -#include -#include -#include -#include -#include - -#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 -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 -struct direct_type; - -template -struct direct_type -{ - static constexpr std::size_t index = std::is_same::value - ? sizeof...(Types) - : direct_type::index; -}; - -template -struct direct_type -{ - static constexpr std::size_t index = invalid_value; -}; - -template -struct convertible_type; - -template -struct convertible_type -{ - static constexpr std::size_t index = std::is_convertible::value - ? sizeof...(Types) - : convertible_type::index; -}; - -template -struct convertible_type -{ - static constexpr std::size_t index = invalid_value; -}; - -template -struct value_traits -{ - using value_type = typename std::remove_reference::type; - static constexpr std::size_t direct_index = direct_type::index; - static constexpr bool is_direct = direct_index != invalid_value; - static constexpr std::size_t index = is_direct ? direct_index : convertible_type::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>::type; -}; - -// check if T is in Types... -template -struct has_type; - -template -struct has_type -{ - static constexpr bool value = std::is_same::value || has_type::value; -}; - -template -struct has_type : std::false_type -{ -}; - -template -struct is_valid_type; - -template -struct is_valid_type -{ - static constexpr bool value = std::is_convertible::value || is_valid_type::value; -}; - -template -struct is_valid_type : std::false_type -{ -}; - -template -struct enable_if_type -{ - using type = R; -}; - -template -struct result_of_unary_visit -{ - using type = typename std::result_of::type; -}; - -template -struct result_of_unary_visit::type> -{ - using type = typename F::result_type; -}; - -template -struct result_of_binary_visit -{ - using type = typename std::result_of::type; -}; - -template -struct result_of_binary_visit::type> -{ - using type = typename F::result_type; -}; - -template -struct static_max; - -template -struct static_max -{ - static const std::size_t value = arg; -}; - -template -struct static_max -{ - static const std::size_t value = arg1 >= arg2 ? static_max::value : static_max::value; -}; - -template -struct variant_helper; - -template -struct variant_helper -{ - VARIANT_INLINE static void destroy(const std::size_t type_index, void* data) - { - if (type_index == sizeof...(Types)) - { - reinterpret_cast(data)->~T(); - } - else - { - variant_helper::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(old_value))); - } - else - { - variant_helper::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(old_value)); - } - else - { - variant_helper::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 -struct unwrapper -{ - static T const& apply_const(T const& obj) { return obj; } - static T& apply(T& obj) { return obj; } -}; - -template -struct unwrapper> -{ - static auto apply_const(recursive_wrapper const& obj) - -> typename recursive_wrapper::type const& - { - return obj.get(); - } - static auto apply(recursive_wrapper& obj) - -> typename recursive_wrapper::type& - { - return obj.get(); - } -}; - -template -struct unwrapper> -{ - static auto apply_const(std::reference_wrapper const& obj) - -> typename std::reference_wrapper::type const& - { - return obj.get(); - } - static auto apply(std::reference_wrapper& obj) - -> typename std::reference_wrapper::type& - { - return obj.get(); - } -}; - -template -struct dispatcher; - -template -struct dispatcher -{ - VARIANT_INLINE static R apply_const(V const& v, F&& f) - { - if (v.template is()) - { - return f(unwrapper::apply_const(v.template get())); - } - else - { - return dispatcher::apply_const(v, std::forward(f)); - } - } - - VARIANT_INLINE static R apply(V& v, F&& f) - { - if (v.template is()) - { - return f(unwrapper::apply(v.template get())); - } - else - { - return dispatcher::apply(v, std::forward(f)); - } - } -}; - -template -struct dispatcher -{ - VARIANT_INLINE static R apply_const(V const& v, F&& f) - { - return f(unwrapper::apply_const(v.template get())); - } - - VARIANT_INLINE static R apply(V& v, F&& f) - { - return f(unwrapper::apply(v.template get())); - } -}; - -template -struct binary_dispatcher_rhs; - -template -struct binary_dispatcher_rhs -{ - VARIANT_INLINE static R apply_const(V const& lhs, V const& rhs, F&& f) - { - if (rhs.template is()) // call binary functor - { - return f(unwrapper::apply_const(lhs.template get()), - unwrapper::apply_const(rhs.template get())); - } - else - { - return binary_dispatcher_rhs::apply_const(lhs, rhs, std::forward(f)); - } - } - - VARIANT_INLINE static R apply(V& lhs, V& rhs, F&& f) - { - if (rhs.template is()) // call binary functor - { - return f(unwrapper::apply(lhs.template get()), - unwrapper::apply(rhs.template get())); - } - else - { - return binary_dispatcher_rhs::apply(lhs, rhs, std::forward(f)); - } - } -}; - -template -struct binary_dispatcher_rhs -{ - VARIANT_INLINE static R apply_const(V const& lhs, V const& rhs, F&& f) - { - return f(unwrapper::apply_const(lhs.template get()), - unwrapper::apply_const(rhs.template get())); - } - - VARIANT_INLINE static R apply(V& lhs, V& rhs, F&& f) - { - return f(unwrapper::apply(lhs.template get()), - unwrapper::apply(rhs.template get())); - } -}; - -template -struct binary_dispatcher_lhs; - -template -struct binary_dispatcher_lhs -{ - VARIANT_INLINE static R apply_const(V const& lhs, V const& rhs, F&& f) - { - if (lhs.template is()) // call binary functor - { - return f(unwrapper::apply_const(lhs.template get()), - unwrapper::apply_const(rhs.template get())); - } - else - { - return binary_dispatcher_lhs::apply_const(lhs, rhs, std::forward(f)); - } - } - - VARIANT_INLINE static R apply(V& lhs, V& rhs, F&& f) - { - if (lhs.template is()) // call binary functor - { - return f(unwrapper::apply(lhs.template get()), - unwrapper::apply(rhs.template get())); - } - else - { - return binary_dispatcher_lhs::apply(lhs, rhs, std::forward(f)); - } - } -}; - -template -struct binary_dispatcher_lhs -{ - VARIANT_INLINE static R apply_const(V const& lhs, V const& rhs, F&& f) - { - return f(unwrapper::apply_const(lhs.template get()), - unwrapper::apply_const(rhs.template get())); - } - - VARIANT_INLINE static R apply(V& lhs, V& rhs, F&& f) - { - return f(unwrapper::apply(lhs.template get()), - unwrapper::apply(rhs.template get())); - } -}; - -template -struct binary_dispatcher; - -template -struct binary_dispatcher -{ - VARIANT_INLINE static R apply_const(V const& v0, V const& v1, F&& f) - { - if (v0.template is()) - { - if (v1.template is()) - { - return f(unwrapper::apply_const(v0.template get()), - unwrapper::apply_const(v1.template get())); // call binary functor - } - else - { - return binary_dispatcher_rhs::apply_const(v0, v1, std::forward(f)); - } - } - else if (v1.template is()) - { - return binary_dispatcher_lhs::apply_const(v0, v1, std::forward(f)); - } - return binary_dispatcher::apply_const(v0, v1, std::forward(f)); - } - - VARIANT_INLINE static R apply(V& v0, V& v1, F&& f) - { - if (v0.template is()) - { - if (v1.template is()) - { - return f(unwrapper::apply(v0.template get()), - unwrapper::apply(v1.template get())); // call binary functor - } - else - { - return binary_dispatcher_rhs::apply(v0, v1, std::forward(f)); - } - } - else if (v1.template is()) - { - return binary_dispatcher_lhs::apply(v0, v1, std::forward(f)); - } - return binary_dispatcher::apply(v0, v1, std::forward(f)); - } -}; - -template -struct binary_dispatcher -{ - VARIANT_INLINE static R apply_const(V const& v0, V const& v1, F&& f) - { - return f(unwrapper::apply_const(v0.template get()), - unwrapper::apply_const(v1.template get())); // call binary functor - } - - VARIANT_INLINE static R apply(V& v0, V& v1, F&& f) - { - return f(unwrapper::apply(v0.template get()), - unwrapper::apply(v1.template get())); // call binary functor - } -}; - -// comparator functors -struct equal_comp -{ - template - bool operator()(T const& lhs, T const& rhs) const - { - return lhs == rhs; - } -}; - -struct less_comp -{ - template - bool operator()(T const& lhs, T const& rhs) const - { - return lhs < rhs; - } -}; - -template -class comparer -{ - public: - explicit comparer(Variant const& lhs) noexcept - : lhs_(lhs) {} - comparer& operator=(comparer const&) = delete; - // visitor - template - bool operator()(T const& rhs_content) const - { - T const& lhs_content = lhs_.template get(); - return Comp()(lhs_content, rhs_content); - } - - private: - Variant const& lhs_; -}; - -// True if Predicate matches for all of the types Ts -template