diff --git a/src/RouteMissionItem/.directory b/src/RouteMissionItem/.directory new file mode 100644 index 0000000000000000000000000000000000000000..fb64c37e492746510948a33681efb302a4e63cac --- /dev/null +++ b/src/RouteMissionItem/.directory @@ -0,0 +1,4 @@ +[Dolphin] +Timestamp=2020,7,8,11,13,28 +Version=4 +ViewMode=1 diff --git a/src/RouteMissionItem/AreaData.cc b/src/RouteMissionItem/AreaData.cc new file mode 100644 index 0000000000000000000000000000000000000000..add9a2b0d2c607f4cb7296e74abd9c1c060deed9 --- /dev/null +++ b/src/RouteMissionItem/AreaData.cc @@ -0,0 +1,137 @@ +#include "WimaPlanData.h" + +AreaData::AreaData(QObject *parent) : QObject(parent) {} + +AreaData::AreaData(const AreaData &other, QObject *parent) + : QObject(parent) { + *this = other; +} + +AreaData &AreaData::operator=(const AreaData &other) { + this->append(other.measurementArea()); + this->append(other.serviceArea()); + this->append(other.joinedArea()); + this->append(other.corridor()); + + return *this; +} + +void AreaData::append(const WimaJoinedAreaData &areaData) { + if (_joinedArea != areaData) { + _joinedArea = areaData; + emit joinedAreaChanged(); + } +} + +void AreaData::append(const WimaServiceAreaData &areaData) { + if (_serviceArea != areaData) { + _serviceArea = areaData; + emit serviceAreaChanged(); + } +} + +void AreaData::append(const WimaCorridorData &areaData) { + if (_corridor != areaData) { + _corridor = areaData; + emit corridorChanged(); + } +} + +void AreaData::append(const WimaMeasurementAreaData &areaData) { + if (_measurementArea != areaData) { + _measurementArea = areaData; + emit measurementAreaChanged(); + + if (_measurementArea.coordinateList().size() > 0) { + setOrigin(_measurementArea.coordinateList().first()); + } else { + setOrigin(QGeoCoordinate()); + } + } +} + +void AreaData::append(const WimaJoinedArea &areaData) { + if (_joinedArea != areaData) { + _joinedArea = areaData; + emit joinedAreaChanged(); + } +} + +void AreaData::append(const WimaArea &areaData) { + if (_serviceArea != areaData) { + _serviceArea = areaData; + emit serviceAreaChanged(); + } +} + +void AreaData::append(const WimaCorridor &areaData) { + if (_corridor != areaData) { + _corridor = areaData; + emit corridorChanged(); + } +} + +void AreaData::append(const WimaMeasurementArea &areaData) { + if (_measurementArea != areaData) { + _measurementArea = areaData; + emit measurementAreaChanged(); + + if (_measurementArea.coordinateList().size() > 0) { + setOrigin(_measurementArea.coordinateList().first()); + } else { + setOrigin(QGeoCoordinate()); + } + } +} + +void AreaData::clear() { *this = AreaData(); } + +const QGeoCoordinate &AreaData::origin() const { return _origin; } + +bool AreaData::isValid() { + return _measurementArea.coordinateList().size() >= 3 && + _serviceArea.coordinateList().size() >= 3 && _origin.isValid(); +} + +const WimaJoinedAreaData &AreaData::joinedArea() const { + return this->_joinedArea; +} + +const WimaServiceAreaData &AreaData::serviceArea() const { + return this->_serviceArea; +} + +const WimaCorridorData &AreaData::corridor() const { + return this->_corridor; +} + +const WimaMeasurementAreaData &AreaData::measurementArea() const { + return this->_measurementArea; +} + +WimaJoinedAreaData &AreaData::joinedArea() { return this->_joinedArea; } + +WimaServiceAreaData &AreaData::serviceArea() { return this->_serviceArea; } + +WimaCorridorData &AreaData::corridor() { return this->_corridor; } + +WimaMeasurementAreaData &AreaData::measurementArea() { + return this->_measurementArea; +} + +bool AreaData::operator==(const AreaData &other) const { + return this->_joinedArea == other._joinedArea && + this->_measurementArea == other._measurementArea && + this->_corridor == other._corridor && + this->_serviceArea == other._serviceArea; +} +bool AreaData::operator!=(const AreaData &other) const { + return !(*this == other); +} + +void AreaData::setOrigin(const QGeoCoordinate &origin) { + if (this->_origin != origin) { + this->_origin = origin; + emit originChanged(); + } +} diff --git a/src/RouteMissionItem/CircularGenerator.cpp b/src/RouteMissionItem/CircularGenerator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..838dae395b68dc2cc7019203fb458542ec4331fd --- /dev/null +++ b/src/RouteMissionItem/CircularGenerator.cpp @@ -0,0 +1,465 @@ +#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/RouteMissionItem/CircularGenerator.h b/src/RouteMissionItem/CircularGenerator.h new file mode 100644 index 0000000000000000000000000000000000000000..d4458a2292ea9b90317d2be338f0848b6fe56503 --- /dev/null +++ b/src/RouteMissionItem/CircularGenerator.h @@ -0,0 +1,63 @@ +#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/RouteMissionItem/GeneratorBase.cc b/src/RouteMissionItem/GeneratorBase.cc new file mode 100644 index 0000000000000000000000000000000000000000..bc315e54b9bd9319d433208f20a75bf29a53c5bd --- /dev/null +++ b/src/RouteMissionItem/GeneratorBase.cc @@ -0,0 +1,31 @@ +#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/RouteMissionItem/GeneratorBase.h b/src/RouteMissionItem/GeneratorBase.h new file mode 100644 index 0000000000000000000000000000000000000000..96605d0543d90c260e02a7127ce86890dbd13c1d --- /dev/null +++ b/src/RouteMissionItem/GeneratorBase.h @@ -0,0 +1,47 @@ +#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/RouteMissionItem/GenericSingelton.cpp b/src/RouteMissionItem/GenericSingelton.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/src/RouteMissionItem/GenericSingelton.h b/src/RouteMissionItem/GenericSingelton.h new file mode 100644 index 0000000000000000000000000000000000000000..93e1ec0d5b684d8059b896f607a4851018b611ae --- /dev/null +++ b/src/RouteMissionItem/GenericSingelton.h @@ -0,0 +1,59 @@ +#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/RouteMissionItem/LinearGenerator.cpp b/src/RouteMissionItem/LinearGenerator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7f5c4b739f038850e1a6165814f4cb157cc7aef0 --- /dev/null +++ b/src/RouteMissionItem/LinearGenerator.cpp @@ -0,0 +1,336 @@ +#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/RouteMissionItem/LinearGenerator.h b/src/RouteMissionItem/LinearGenerator.h new file mode 100644 index 0000000000000000000000000000000000000000..ba63cf51a322f7c6e3d0b3cdc30ae3a6b5c6183e --- /dev/null +++ b/src/RouteMissionItem/LinearGenerator.h @@ -0,0 +1,46 @@ +#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/RouteMissionItem/NemoInterface.cpp b/src/RouteMissionItem/NemoInterface.cpp new file mode 100644 index 0000000000000000000000000000000000000000..dd07f8d3999ed8127796f5562e89f869698951d3 --- /dev/null +++ b/src/RouteMissionItem/NemoInterface.cpp @@ -0,0 +1,468 @@ +#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/RouteMissionItem/NemoInterface.h b/src/RouteMissionItem/NemoInterface.h new file mode 100644 index 0000000000000000000000000000000000000000..459f02917dae3f177ef96e5579db979054c80c31 --- /dev/null +++ b/src/RouteMissionItem/NemoInterface.h @@ -0,0 +1,57 @@ +#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/RouteMissionItem/OptimisationTools.cc b/src/RouteMissionItem/OptimisationTools.cc new file mode 100644 index 0000000000000000000000000000000000000000..0d03739df441d601eb955d9c7ff7bd85e3f762c0 --- /dev/null +++ b/src/RouteMissionItem/OptimisationTools.cc @@ -0,0 +1,105 @@ +#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/RouteMissionItem/OptimisationTools.h b/src/RouteMissionItem/OptimisationTools.h new file mode 100644 index 0000000000000000000000000000000000000000..11c37431dc27d7ae63663b741889d775d5dd14d1 --- /dev/null +++ b/src/RouteMissionItem/OptimisationTools.h @@ -0,0 +1,12 @@ +#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/RouteMissionItem/RouteComplexItem.cc b/src/RouteMissionItem/RouteComplexItem.cc new file mode 100644 index 0000000000000000000000000000000000000000..c5c2ffc2cf17d91e182e7d6ba935af1699590684 --- /dev/null +++ b/src/RouteMissionItem/RouteComplexItem.cc @@ -0,0 +1,646 @@ +#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/RouteMissionItem/RouteComplexItem.h b/src/RouteMissionItem/RouteComplexItem.h new file mode 100644 index 0000000000000000000000000000000000000000..83981c9c457d859fec9338a2fdb4492282210d07 --- /dev/null +++ b/src/RouteMissionItem/RouteComplexItem.h @@ -0,0 +1,127 @@ +#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, REVERT_PATH, CHANGE_VARIANT }; + +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 revert_path(void); + + // Property getters + const AreaData &planData() const; + Fact *variant(); + QStringList variantNames() const; + bool calculating() const; + + // Overrides + virtual bool load(const QJsonObject &complexObject, int sequenceNumber, + QString &errorString) override final; + virtual void save(QJsonArray &planItems) override final; + virtual QString mapVisualQML(void) const 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 + AreaData _areaData; + + // Generators + QList _generatorList; + QStringList _generatorNameList; + PtrGenerator _pGenerator; + + // Routing. + QVector _variantVector; + PtrWorker _pWorker; +}; diff --git a/src/RouteMissionItem/RoutingThread.cpp b/src/RouteMissionItem/RoutingThread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f45bb3c0349231b9e91a4203ef6c38a7ad95a847 --- /dev/null +++ b/src/RouteMissionItem/RoutingThread.cpp @@ -0,0 +1,129 @@ +#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/RouteMissionItem/RoutingThread.h b/src/RouteMissionItem/RoutingThread.h new file mode 100644 index 0000000000000000000000000000000000000000..bc5a3f11389c15e847db2438e26abe9c316adc93 --- /dev/null +++ b/src/RouteMissionItem/RoutingThread.h @@ -0,0 +1,63 @@ +#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/RouteMissionItem/WimaPlaner.cc b/src/RouteMissionItem/WimaPlaner.cc new file mode 100644 index 0000000000000000000000000000000000000000..3cc787ad1c732a043b192f0d85087bcea919a34b --- /dev/null +++ b/src/RouteMissionItem/WimaPlaner.cc @@ -0,0 +1,1002 @@ +#include "WimaPlaner.h" + +#include "MissionController.h" +#include "MissionSettingsItem.h" +#include "PlanMasterController.h" +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" +#include "QGCMapPolygon.h" +#include "SimpleMissionItem.h" + +#include "Geometry/GeoUtilities.h" +#include "Geometry/PlanimetryCalculus.h" +#include "OptimisationTools.h" + +#include "CircularSurvey.h" +#include "Geometry/WimaArea.h" +#include "Geometry/WimaAreaData.h" +#include "WimaBridge.h" + +#include "WimaStateMachine.h" +using namespace wima_planer_detail; + +#include + +QGC_LOGGING_CATEGORY(WimaPlanerLog, "WimaPlanerLog") + +class CommandRAII { + std::function f; + +public: + CommandRAII(const std::function &fun) : f(fun) {} + ~CommandRAII() { f(); } +}; + +const char *WimaPlaner::wimaFileExtension = "wima"; +const char *WimaPlaner::areaItemsName = "AreaItems"; +const char *WimaPlaner::missionItemsName = "MissionItems"; + +WimaPlaner::WimaPlaner(QObject *parent) + : QObject(parent), _masterController(nullptr), _missionController(nullptr), + _currentAreaIndex(-1), _joinedArea(this), _survey(nullptr), + _nemoInterface(this), _stateMachine(new WimaStateMachine), + _areasMonitored(false), _missionControllerMonitored(false), + _progressLocked(false), _synchronized(false) { + + connect(this, &WimaPlaner::currentPolygonIndexChanged, this, + &WimaPlaner::updatePolygonInteractivity); + + // Monitoring. + enableAreaMonitoring(); + // Mission controller not set at this point. Not enabling monitoring. + +#ifndef NDEBUG + // for debugging and testing purpose, remove if not needed anymore + connect(&_autoLoadTimer, &QTimer::timeout, this, + &WimaPlaner::autoLoadMission); + _autoLoadTimer.setSingleShot(true); + _autoLoadTimer.start(300); +#endif + + // NemoInterface + connect(&this->_nemoInterface, &NemoInterface::progressChanged, this, + &WimaPlaner::nemoInterfaceProgressChangedHandler); + + // StateMachine + connect(this->_stateMachine.get(), &WimaStateMachine::upToDateChanged, this, + &WimaPlaner::needsUpdateChanged); + connect(this->_stateMachine.get(), &WimaStateMachine::surveyReadyChanged, this, + &WimaPlaner::readyForSynchronizationChanged); + connect(this->_stateMachine.get(), &WimaStateMachine::surveyReadyChanged, this, + &WimaPlaner::surveyReadyChanged); +} + +WimaPlaner::~WimaPlaner() {} + +PlanMasterController *WimaPlaner::masterController() { + return _masterController; +} + +MissionController *WimaPlaner::missionController() { + return _missionController; +} + +QmlObjectListModel *WimaPlaner::visualItems() { return &_visualItems; } + +int WimaPlaner::currentPolygonIndex() const { return _currentAreaIndex; } + +QString WimaPlaner::currentFile() const { return _currentFile; } + +QStringList WimaPlaner::loadNameFilters() const { + QStringList filters; + + filters << tr("Supported types (*.%1 *.%2)") + .arg(wimaFileExtension) + .arg(AppSettings::planFileExtension) + << tr("All Files (*.*)"); + return filters; +} + +QStringList WimaPlaner::saveNameFilters() const { + QStringList filters; + + filters << tr("Supported types (*.%1 *.%2)") + .arg(wimaFileExtension) + .arg(AppSettings::planFileExtension); + return filters; +} + +QString WimaPlaner::fileExtension() const { return wimaFileExtension; } + +QGeoCoordinate WimaPlaner::joinedAreaCenter() const { + return _joinedArea.center(); +} + +NemoInterface *WimaPlaner::nemoInterface() { return &_nemoInterface; } + +void WimaPlaner::setMasterController(PlanMasterController *masterC) { + if (_masterController != masterC) { + _masterController = masterC; + emit masterControllerChanged(); + } +} + +void WimaPlaner::setMissionController(MissionController *missionC) { + if (_missionController != missionC) { + disableMissionControllerMonitoring(); + _missionController = missionC; + enableMissionControllerMonitoring(); + emit missionControllerChanged(); + } +} + +void WimaPlaner::setCurrentPolygonIndex(int index) { + if (index >= 0 && index < _visualItems.count() && + index != _currentAreaIndex) { + _currentAreaIndex = index; + + emit currentPolygonIndexChanged(index); + } +} + +void WimaPlaner::setProgressLocked(bool l) { + if (this->_progressLocked != l) { + this->_progressLocked = l; + emit progressLockedChanged(); + if (!this->_progressLocked) { + if (this->_measurementArea.setProgress(this->_nemoInterface.progress())) + this->_update(); + } + } +} + +bool WimaPlaner::synchronized() { return _synchronized; } + +bool WimaPlaner::needsUpdate() { return !this->_stateMachine->upToDate(); } + +bool WimaPlaner::readyForSynchronization() { + return this->_stateMachine->surveyReady(); +} + +bool WimaPlaner::surveyReady() { return this->_stateMachine->surveyReady(); } + +bool WimaPlaner::progressLocked() { return this->_progressLocked; } + +void WimaPlaner::removeArea(int index) { + if (index >= 0 && index < _visualItems.count()) { + WimaArea *area = qobject_cast(_visualItems.removeAt(index)); + + if (area == nullptr) { + qCWarning(WimaPlanerLog) + << "removeArea(): nullptr catched, internal error."; + return; + } + area->clear(); + area->borderPolygon()->clear(); + + emit visualItemsChanged(); + + if (_visualItems.count() == 0) { + // this branch is reached if all items are removed + // to guarentee proper behavior, _currentAreaIndex must be set to a + // invalid value, as on constructor init. + resetAllInteractive(); + _currentAreaIndex = -1; + return; + } + + if (_currentAreaIndex >= _visualItems.count()) { + setCurrentPolygonIndex(_visualItems.count() - 1); + } else { + updatePolygonInteractivity(_currentAreaIndex); + } + } else { + qCWarning(WimaPlanerLog) << "removeArea(): Index out of bounds!"; + } +} + +bool WimaPlaner::addMeasurementArea() { + if (!_visualItems.contains(&_measurementArea)) { + _visualItems.append(&_measurementArea); + + int newIndex = _visualItems.count() - 1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +bool WimaPlaner::addServiceArea() { + if (!_visualItems.contains(&_serviceArea)) { + _visualItems.append(&_serviceArea); + + int newIndex = _visualItems.count() - 1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +bool WimaPlaner::addCorridor() { + if (!_visualItems.contains(&_corridor)) { + _visualItems.append(&_corridor); + + int newIndex = _visualItems.count() - 1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +void WimaPlaner::removeAll() { + bool changesApplied = false; + // Delete Pointers. + while (_visualItems.count() > 0) { + removeArea(0); + changesApplied = true; + } + + _measurementArea = WimaMeasurementArea(); + _joinedArea = WimaJoinedArea(); + _serviceArea = WimaServiceArea(); + _corridor = WimaCorridor(); + + // Remove missions items. + _missionController->removeAll(); + _currentFile = ""; + _survey = nullptr; + + emit currentFileChanged(); + if (changesApplied) + emit visualItemsChanged(); +} + +void WimaPlaner::update() { this->_update(); } + +void WimaPlaner::_update() { + setSynchronized(false); + + switch (this->_stateMachine->state()) { + case STATE::NEEDS_INIT: { + if (this->_measurementArea.ready()) { + this->_stateMachine->updateState(EVENT::INIT_DONE); + this->_update(); + } else { + this->_stateMachine->updateState(EVENT::M_AREA_NOT_READY); + } + } break; + + case STATE::WAITING_FOR_TILE_UPDATE: { + } break; + + case STATE::NEEDS_J_AREA_UPDATE: { + // check if at least service area and measurement area are available + if (_visualItems.indexOf(&_serviceArea) == -1 || + _visualItems.indexOf(&_measurementArea) == -1) + return; + + // Check if polygons have at least three vertices + if (_serviceArea.count() < 3) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Service area has less than three vertices.")); + return; + } + + if (_measurementArea.count() < 3) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), + tr("Measurement area has less than three vertices.")); + return; + } + + // Check for simple polygons + if (!_serviceArea.isSimplePolygon()) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Service area is not a simple polygon. Only " + "simple polygons allowed.")); + return; + } + + if (!_corridor.isSimplePolygon() && _corridor.count() > 0) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Corridor is not a simple polygon. Only simple " + "polygons allowed.")); + return; + } + + if (!_measurementArea.isSimplePolygon()) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Measurement area is not a simple polygon. Only " + "simple polygons allowed.")); + return; + } + + if (!_serviceArea.containsCoordinate(_serviceArea.depot())) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Depot not inside service area.")); + return; + } + + // Join areas. + _joinedArea.setPath(_serviceArea.path()); + if (_corridor.count() >= 3) { + _joinedArea.join(_corridor); + } + if (!_joinedArea.join(_measurementArea)) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), + tr("Not able to join areas. Service and measurement area" + " must be overlapping, or connected through a " + "corridor.")); + return; + } + this->_stateMachine->updateState(EVENT::J_AREA_UPDATED); + this->_update(); + } break; // STATE::NEEDS_J_AREA_UPDATE + + case STATE::NEEDS_SURVEY_UPDATE: { + // Need to insert Survey? + QmlObjectListModel *missionItems = _missionController->visualItems(); + int surveyIndex = missionItems->indexOf(_survey); + // Create survey item if not yet present. + if (surveyIndex < 0) { + _missionController->insertComplexMissionItem( + CircularSurvey::name, + _measurementArea.center(), missionItems->count()); + _survey = qobject_cast( + missionItems->get(missionItems->count() - 1)); + + if (_survey == nullptr) { + qCWarning(WimaPlanerLog) << "_survey == nullptr"; + return; + } + + // establish connections + connect(_survey, &CircularSurvey::visualTransectPointsChanged, this, + &WimaPlaner::CSVisualTransectPointsChangedHandler); + connect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); + } + + (void)toPlanData(this->_survey->planData()); + } break; // STATE::NEEDS_SURVEY_UPDATE + + case STATE::WAITING_FOR_SURVEY_UPDATE: { + } break; + + case STATE::NEEDS_PATH_UPDATE: { + // Check if survey is present. + QmlObjectListModel *missionItems = _missionController->visualItems(); + int surveyIndex = missionItems->indexOf(_survey); + if (surveyIndex < 0) { + this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED); + this->_update(); + } else { + + // Remove old arrival and return path. + int size = missionItems->count(); + for (int i = surveyIndex + 1; i < size; i++) + _missionController->removeVisualItem(surveyIndex + 1); + for (int i = surveyIndex - 1; i > 1; i--) + _missionController->removeVisualItem(i); + + // set home position to serArea center + MissionSettingsItem *settingsItem = + qobject_cast(missionItems->get(0)); + if (settingsItem == nullptr) { + qCWarning(WimaPlanerLog) << "update(): settingsItem == nullptr"; + return; + } + + // set altitudes + auto depot = _serviceArea.depot(); + depot.setAltitude(0); + settingsItem->setCoordinate(depot); + + // set takeoff position + _missionController->insertTakeoffItem(depot, 1, false); + + if (_survey->visualTransectPoints().size() == 0) { + qCWarning(WimaPlanerLog) << "update(): survey no points"; + return; + } + + // calculate path from take off to survey + QGeoCoordinate start = depot; + QGeoCoordinate end = _survey->coordinate(); + QVector path; + if (!shortestPath(start, end, path)) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Path Error"), tr("Not able to calculate path from " + "depot position to measurement area. Please " + "double check area and route parameters.")); + return; + } + + for (int i = 1; i < path.count() - 1; i++) { + (void)_missionController->insertSimpleMissionItem( + path[i], missionItems->count() - 1); + } + + // calculate return path + start = _survey->exitCoordinate(); + end = depot; + path.clear(); + if (!shortestPath(start, end, path)) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Path Error"), tr("Not able to calculate path from " + "measurement area to depot position. Please " + "double check area and route parameters.")); + return; + } + + for (int i = 1; i < path.count() - 1; i++) { + (void)_missionController->insertSimpleMissionItem( + path[i], missionItems->count()); + } + + // Add waypoint (rover ignores land command). + (void)_missionController->insertSimpleMissionItem(depot, + missionItems->count()); + // create land position item + (void)_missionController->insertLandItem(depot, missionItems->count(), + true); + + auto surveyIndex = _missionController->visualItems()->indexOf(_survey); + _missionController->setCurrentPlanViewSeqNum(surveyIndex, true); + + this->_stateMachine->updateState(EVENT::PATH_UPDATED); + } + } break; // STATE::NEEDS_PATH_UPDATE + + case STATE::UP_TO_DATE: { + } break; // STATE::UP_TO_DATE + + } // switch +} + +void WimaPlaner::CSDestroyedHandler() { + this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED); +} + +void WimaPlaner::CSVisualTransectPointsChangedHandler() { + if (this->_survey && this->_survey->calculating()){ + this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED); + + } else { + this->_stateMachine->updateState(EVENT::SURVEY_UPDATED); + this->_update(); + } +} + +void WimaPlaner::mAreaPathChangedHandler() { + this->_stateMachine->updateState(EVENT::M_AREA_PATH_CHANGED); +} + +void WimaPlaner::mAreaTilesChangedHandler() { + this->_nemoInterface.setTileData(this->_measurementArea.tileData()); + this->_stateMachine->updateState(EVENT::M_AREA_TILES_CHANGED); +} + +void WimaPlaner::mAreaProgressChangedHandler() { + this->_stateMachine->updateState(EVENT::M_AREA_PROGRESS_CHANGED); +} + +void WimaPlaner::mAreaProgressAcceptedHandler() { this->_update(); } + +void WimaPlaner::mAreaReadyChangedHandler() { + if (this->_measurementArea.ready()) { + this->_stateMachine->updateState(EVENT::M_AREA_READY); + } else { + this->_stateMachine->updateState(EVENT::M_AREA_NOT_READY); + } +} + +void WimaPlaner::sAreaPathChangedHandler() { + this->_stateMachine->updateState(EVENT::S_AREA_PATH_CHANGED); +} + +void WimaPlaner::corridorPathChangedHandler() { + this->_stateMachine->updateState(EVENT::CORRIDOR_PATH_CHANGED); +} + +void WimaPlaner::depotChangedHandler() { + this->_stateMachine->updateState(EVENT::DEPOT_CHANGED); +} + +void WimaPlaner::missionControllerVisualItemsChangedHandler() { + // Search for survey. + auto surveyIndex = _missionController->visualItems()->indexOf(_survey); + if (surveyIndex < 0) { + // survey not found. + this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED); + } else { + this->_stateMachine->updateState(EVENT::PATH_CHANGED); + } +} + +void WimaPlaner::missionControllerWaypointPathChangedHandler() { + missionControllerVisualItemsChangedHandler(); +} + +void WimaPlaner::missionControllerNewItemsFromVehicleHandler() { + this->_stateMachine->updateState(EVENT::MISSION_ITEMS_DESTROYED); +} + +void WimaPlaner::missionControllerMissionItemCountChangedHandler() { + missionControllerVisualItemsChangedHandler(); +} + +void WimaPlaner::nemoInterfaceProgressChangedHandler() { + auto p = this->_nemoInterface.progress(); + WimaBridge::instance()->setProgress(p); + + if (!progressLocked()) { + this->_measurementArea.setProgress(p); + this->_update(); + } +} + +void WimaPlaner::saveToCurrent() { saveToFile(_currentFile); } + +void WimaPlaner::saveToFile(const QString &filename) { + if (filename.isEmpty()) { + return; + } + + QString planFilename = filename; + if (!QFileInfo(filename).fileName().contains(".")) { + planFilename += QString(".%1").arg(wimaFileExtension); + } + + QFile file(planFilename); + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Save Error"), + tr("Plan save error %1 : %2").arg(filename).arg(file.errorString())); + _currentFile.clear(); + emit currentFileChanged(); + } else { + FileType fileType = FileType::WimaFile; + if (planFilename.contains(QString(".%1").arg(wimaFileExtension))) { + fileType = FileType::WimaFile; + } else if (planFilename.contains( + QString(".%1").arg(AppSettings::planFileExtension))) { + fileType = FileType::PlanFile; + } else { + if (planFilename.contains(".")) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Save Error"), tr("File format not supported")); + } else { + qgcApp()->warningMessageBoxOnMainThread( + tr("Save Error"), tr("File without file extension not accepted.")); + return; + } + } + + QJsonDocument saveDoc = saveToJson(fileType); + file.write(saveDoc.toJson()); + if (_currentFile != planFilename) { + _currentFile = planFilename; + emit currentFileChanged(); + } + } +} + +bool WimaPlaner::loadFromCurrent() { return loadFromFile(_currentFile); } + +bool WimaPlaner::loadFromFile(const QString &filename) { + // Remove obsolete connections. + disableAreaMonitoring(); + disableMissionControllerMonitoring(); + CommandRAII onExit([this] { + this->enableAreaMonitoring(); + this->enableMissionControllerMonitoring(); + }); + + // disconnect old survey + if (_survey != nullptr) { + disconnect(_survey, &CircularSurvey::visualTransectPointsChanged, this, + &WimaPlaner::CSVisualTransectPointsChangedHandler); + disconnect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); + } + + setSynchronized(false); + + // Precondition. + QString errorString; + QString errorMessage = + tr("Error loading Plan file (%1). %2").arg(filename).arg("%1"); + + if (filename.isEmpty()) { + return false; + } + + QFileInfo fileInfo(filename); + QFile file(filename); + + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + errorString = file.errorString() + QStringLiteral(" ") + filename; + qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"), + errorMessage.arg(errorString)); + return false; + } + + if (fileInfo.suffix() == wimaFileExtension) { + QJsonDocument jsonDoc; + QByteArray bytes = file.readAll(); + + if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) { + qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"), + errorMessage.arg(errorString)); + return false; + } + + QJsonObject json = jsonDoc.object(); + // AreaItems + QJsonArray areaArray = json[areaItemsName].toArray(); + _visualItems.clear(); + + int validAreaCounter = 0; + for (int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) { + QJsonObject jsonArea = areaArray[i].toObject(); + + if (jsonArea.contains(WimaArea::areaTypeName) && + jsonArea[WimaArea::areaTypeName].isString()) { + if (jsonArea[WimaArea::areaTypeName] == + WimaMeasurementArea::WimaMeasurementAreaName) { + bool success = _measurementArea.loadFromJson(jsonArea, errorString); + + if (!success) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Load Error"), errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_measurementArea); + emit visualItemsChanged(); + } else if (jsonArea[WimaArea::areaTypeName] == + WimaServiceArea::wimaServiceAreaName) { + bool success = _serviceArea.loadFromJson(jsonArea, errorString); + + if (!success) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Load Error"), errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_serviceArea); + emit visualItemsChanged(); + } else if (jsonArea[WimaArea::areaTypeName] == + WimaCorridor::WimaCorridorName) { + bool success = _corridor.loadFromJson(jsonArea, errorString); + + if (!success) { + qgcApp()->warningMessageBoxOnMainThread( + tr("Load Error"), errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_corridor); + emit visualItemsChanged(); + } else { + errorString += + QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName)); + qgcApp()->warningMessageBoxOnMainThread( + tr("Load Error"), errorMessage.arg(errorString)); + return false; + } + } else { + errorString += QString(tr("Invalid or non existing entry for %s.\n") + .arg(WimaArea::areaTypeName)); + return false; + } + } + + _currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(), + fileInfo.completeBaseName().toLocal8Bit().data(), + wimaFileExtension); + + emit currentFileChanged(); + + QJsonObject missionObject = json[missionItemsName].toObject(); + + QJsonDocument missionJsonDoc = QJsonDocument(missionObject); + // create temporary file with missionItems + QFile temporaryFile; + QString cropedFileName = filename.section("/", 0, -2); + QString temporaryFileName; + for (int i = 0;; i++) { + temporaryFileName = + cropedFileName + + QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension); + + if (!QFile::exists(temporaryFileName)) { + temporaryFile.setFileName(temporaryFileName); + if (temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text)) { + break; + } + } + + if (i > 1000) { + qCWarning(WimaPlanerLog) + << "loadFromFile(): not able to create temporary file."; + return false; + } + } + + temporaryFile.write(missionJsonDoc.toJson()); + temporaryFile.close(); + + // load from temporary file + _masterController->loadFromFile(temporaryFileName); + QmlObjectListModel *missionItems = _missionController->visualItems(); + _survey = nullptr; + for (int i = 0; i < missionItems->count(); i++) { + _survey = missionItems->value(i); + if (_survey != nullptr) { + connect(_survey, &CircularSurvey::visualTransectPointsChanged, this, + &WimaPlaner::CSVisualTransectPointsChangedHandler); + connect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); + break; + } + } + + // remove temporary file + if (!temporaryFile.remove()) { + qCWarning(WimaPlanerLog) + << "WimaPlaner::loadFromFile(): not able to remove " + "temporary file."; + } + return true; + } else { + errorString += QString(tr("File extension not supported.\n")); + qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"), + errorMessage.arg(errorString)); + return false; + } +} + +void WimaPlaner::updatePolygonInteractivity(int index) { + if (index >= 0 && index < _visualItems.count()) { + resetAllInteractive(); + WimaArea *interactivePoly = + qobject_cast(_visualItems.get(index)); + if (interactivePoly != nullptr) + interactivePoly->setWimaAreaInteractive(true); + } +} + +void WimaPlaner::synchronize() { + if (readyForSynchronization()) { + AreaData planData; + if (toPlanData(planData)) { + WimaBridge::instance()->setPlanData(planData); + setSynchronized(true); + } else { + qCWarning(WimaPlanerLog) << "error creating plan data."; + } + } +} + +bool WimaPlaner::shortestPath(const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path) { + using namespace GeoUtilities; + using namespace PolygonCalculus; + QPolygonF polygon2D; + toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D); + QPointF start2D(0, 0); + QPointF end2D; + QVector path2D; + toCartesian(destination, start, end2D); + bool retVal = + PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); + toGeoList(path2D, /*origin*/ start, path); + + return retVal; +} + +void WimaPlaner::setSynchronized(bool s) { + if (this->_synchronized != s) { + this->_synchronized = s; + emit this->synchronizedChanged(); + } +} + +void WimaPlaner::enableAreaMonitoring() { + if (!areasMonitored()) { + connect(&this->_measurementArea, &WimaArea::pathChanged, this, + &WimaPlaner::mAreaPathChangedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, this, + &WimaPlaner::mAreaTilesChangedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::progressChanged, + this, &WimaPlaner::mAreaProgressChangedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted, + this, &WimaPlaner::mAreaProgressAcceptedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::readyChanged, this, + &WimaPlaner::mAreaReadyChangedHandler); + connect(&this->_serviceArea, &WimaArea::pathChanged, this, + &WimaPlaner::sAreaPathChangedHandler); + connect(&this->_serviceArea, &WimaServiceArea::depotChanged, this, + &WimaPlaner::depotChangedHandler); + connect(&this->_corridor, &WimaArea::pathChanged, this, + &WimaPlaner::corridorPathChangedHandler); + this->_areasMonitored = true; + } +} + +void WimaPlaner::disableAreaMonitoring() { + if (areasMonitored()) { + disconnect(&this->_measurementArea, &WimaArea::pathChanged, this, + &WimaPlaner::mAreaPathChangedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, + this, &WimaPlaner::mAreaTilesChangedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::progressChanged, + this, &WimaPlaner::mAreaProgressChangedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted, + this, &WimaPlaner::mAreaProgressAcceptedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::readyChanged, + this, &WimaPlaner::mAreaReadyChangedHandler); + disconnect(&this->_serviceArea, &WimaArea::pathChanged, this, + &WimaPlaner::sAreaPathChangedHandler); + disconnect(&this->_serviceArea, &WimaServiceArea::depotChanged, this, + &WimaPlaner::depotChangedHandler); + disconnect(&this->_corridor, &WimaArea::pathChanged, this, + &WimaPlaner::corridorPathChangedHandler); + this->_areasMonitored = false; + } +} + +void WimaPlaner::enableMissionControllerMonitoring() { + if (!missionControllerMonitored() && this->missionController() != nullptr) { + connect(this->missionController(), &MissionController::visualItemsChanged, + this, &WimaPlaner::missionControllerVisualItemsChangedHandler); + connect(this->missionController(), &MissionController::waypointPathChanged, + this, &WimaPlaner::missionControllerWaypointPathChangedHandler); + connect(this->missionController(), &MissionController::newItemsFromVehicle, + this, &WimaPlaner::missionControllerNewItemsFromVehicleHandler); + connect(this->missionController(), + &MissionController::missionItemCountChanged, this, + &WimaPlaner::missionControllerMissionItemCountChangedHandler); + this->_missionControllerMonitored = true; + } +} + +void WimaPlaner::disableMissionControllerMonitoring() { + if (missionControllerMonitored() && this->missionController() != nullptr) { + disconnect(this->missionController(), + &MissionController::visualItemsChanged, this, + &WimaPlaner::missionControllerVisualItemsChangedHandler); + disconnect(this->missionController(), + &MissionController::waypointPathChanged, this, + &WimaPlaner::missionControllerWaypointPathChangedHandler); + disconnect(this->missionController(), + &MissionController::newItemsFromVehicle, this, + &WimaPlaner::missionControllerNewItemsFromVehicleHandler); + disconnect(this->missionController(), + &MissionController::missionItemCountChanged, this, + &WimaPlaner::missionControllerMissionItemCountChangedHandler); + this->_missionControllerMonitored = false; + } +} + +bool WimaPlaner::areasMonitored() { return this->_areasMonitored; } + +bool WimaPlaner::missionControllerMonitored() { + return this->_missionControllerMonitored; +} + +void WimaPlaner::resetAllInteractive() { + // Marks all areas as inactive (area.interactive == false) + int itemCount = _visualItems.count(); + if (itemCount > 0) { + for (int i = 0; i < itemCount; i++) { + WimaArea *iteratorPoly = qobject_cast(_visualItems.get(i)); + iteratorPoly->setWimaAreaInteractive(false); + } + } +} + +void WimaPlaner::setInteractive() { + updatePolygonInteractivity(_currentAreaIndex); +} + +/*! + * \fn WimaPlanData WimaPlaner::toPlanData() + * + * Returns a \c WimaPlanData object containing information about the current + * mission. The \c WimaPlanData object holds only the data which is relevant + * for the \c WimaController class. Should only be called if update() was + * successful. + * + * \sa WimaController, WimaPlanData + */ +bool WimaPlaner::toPlanData(AreaData &planData) { + planData.append(_measurementArea); + planData.append(_serviceArea); + planData.append(_corridor); + planData.append(_joinedArea); + return planData.isValid(); +} + +#ifndef NDEBUG +void WimaPlaner::autoLoadMission() { + loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/" + "KlingenbachTest.wima"); + synchronize(); +} +#endif + +QJsonDocument WimaPlaner::saveToJson(FileType fileType) { + /// This function save all areas (of WimaPlaner) and all mission items (of + /// MissionController) to a QJsonDocument + /// @param fileType is either WimaFile or PlanFile (enum), if fileType == + /// PlanFile only mission items are stored + QJsonObject json; + + if (fileType == FileType::WimaFile) { + QJsonArray jsonArray; + + for (int i = 0; i < _visualItems.count(); i++) { + QJsonObject json; + + WimaArea *area = qobject_cast(_visualItems.get(i)); + + if (area == nullptr) { + qCWarning(WimaPlanerLog) << "saveing, area == nullptr!"; + return QJsonDocument(); + } + + // check the type of area, create and append the JsonObject to the + // JsonArray once determined + WimaMeasurementArea *opArea = qobject_cast(area); + if (opArea != nullptr) { + opArea->saveToJson(json); + jsonArray.append(json); + continue; + } + + WimaServiceArea *serArea = qobject_cast(area); + if (serArea != nullptr) { + serArea->saveToJson(json); + jsonArray.append(json); + continue; + } + + WimaCorridor *corridor = qobject_cast(area); + if (corridor != nullptr) { + corridor->saveToJson(json); + jsonArray.append(json); + continue; + } + + // if non of the obove branches was trigger, type must be WimaArea + area->saveToJson(json); + jsonArray.append(json); + } + + json[areaItemsName] = jsonArray; + json[missionItemsName] = _masterController->saveToJson().object(); + + return QJsonDocument(json); + } else if (fileType == FileType::PlanFile) { + return _masterController->saveToJson(); + } + + return QJsonDocument(json); +} diff --git a/src/RouteMissionItem/WimaPlaner.h b/src/RouteMissionItem/WimaPlaner.h new file mode 100644 index 0000000000000000000000000000000000000000..2b02129535d0d5a4a0c9dca158f06279008e5b1a --- /dev/null +++ b/src/RouteMissionItem/WimaPlaner.h @@ -0,0 +1,196 @@ +#pragma once + +#include "QmlObjectListModel.h" +#include +#include +#include + +#include "Geometry/WimaCorridor.h" +#include "Geometry/WimaCorridorData.h" +#include "Geometry/WimaJoinedArea.h" +#include "Geometry/WimaJoinedAreaData.h" +#include "Geometry/WimaMeasurementArea.h" +#include "Geometry/WimaMeasurementAreaData.h" +#include "Geometry/WimaServiceArea.h" +#include "Geometry/WimaServiceAreaData.h" +#include "WimaPlanData.h" + +#include "Snake/NemoInterface.h" + +#include "JsonHelper.h" + +class MissionController; +class PlanMasterController; + +namespace wima_planer_detail { +class WimaStateMachine; +} + +class WimaPlaner : public QObject { + Q_OBJECT + + enum FileType { WimaFile, PlanFile }; + +public: + WimaPlaner(QObject *parent = nullptr); + ~WimaPlaner(); + template WimaPlaner(T t, QObject *parent = nullptr) = delete; + template WimaPlaner(T t) = delete; + + Q_PROPERTY(PlanMasterController *masterController READ masterController WRITE + setMasterController NOTIFY masterControllerChanged) + Q_PROPERTY(MissionController *missionController READ missionController WRITE + setMissionController NOTIFY missionControllerChanged) + Q_PROPERTY(QmlObjectListModel *visualItems READ visualItems NOTIFY + visualItemsChanged) + Q_PROPERTY(int currentPolygonIndex READ currentPolygonIndex WRITE + setCurrentPolygonIndex NOTIFY currentPolygonIndexChanged) + Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged) + Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT) + Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT) + Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) + Q_PROPERTY(QGeoCoordinate joinedAreaCenter READ joinedAreaCenter CONSTANT) + Q_PROPERTY(NemoInterface *nemoInterface READ nemoInterface CONSTANT) + Q_PROPERTY(bool synchronized READ synchronized NOTIFY synchronizedChanged) + Q_PROPERTY(bool needsUpdate READ needsUpdate NOTIFY needsUpdateChanged) + Q_PROPERTY(bool readyForSynchronization READ readyForSynchronization NOTIFY + readyForSynchronizationChanged) + Q_PROPERTY(bool surveyReady READ surveyReady NOTIFY surveyReadyChanged) + Q_PROPERTY(bool progressLocked READ progressLocked WRITE setProgressLocked + NOTIFY progressLockedChanged) + + // Property accessors + PlanMasterController *masterController(void); + MissionController *missionController(void); + QmlObjectListModel *visualItems(void); + int currentPolygonIndex(void) const; + QString currentFile(void) const; + QStringList loadNameFilters(void) const; + QStringList saveNameFilters(void) const; + QString fileExtension(void) const; + QGeoCoordinate joinedAreaCenter(void) const; + NemoInterface *nemoInterface(void); + bool synchronized(); + bool needsUpdate(); + bool readyForSynchronization(); + bool surveyReady(); + bool progressLocked(); + + // Property setters + void setMasterController(PlanMasterController *masterController); + void setMissionController(MissionController *missionController); + /// Sets the integer index pointing to the current polygon. Current polygon is + /// set interactive. + void setCurrentPolygonIndex(int index); + void setProgressLocked(bool l); + + Q_INVOKABLE bool addMeasurementArea(); + /// Removes an area from _visualItems + /// @param index Index of the area to be removed + Q_INVOKABLE void removeArea(int index); + Q_INVOKABLE bool addServiceArea(); + Q_INVOKABLE bool addCorridor(); + /// Remove all areas from WimaPlaner and all mission items from + /// MissionController + Q_INVOKABLE void removeAll(); + /// Recalculates vehicle corridor, flight path, etc. + Q_INVOKABLE void update(); + /// Pushes the generated mission data to the wimaController. + Q_INVOKABLE void synchronize(); + Q_INVOKABLE void saveToCurrent(); + Q_INVOKABLE void saveToFile(const QString &filename); + Q_INVOKABLE bool loadFromCurrent(); + Q_INVOKABLE bool loadFromFile(const QString &filename); + Q_INVOKABLE void resetAllInteractive(void); + Q_INVOKABLE void setInteractive(void); + + QJsonDocument saveToJson(FileType fileType); + // static Members + static const char *wimaFileExtension; + static const char *areaItemsName; + static const char *missionItemsName; + +signals: + void masterControllerChanged(void); + void missionControllerChanged(void); + void visualItemsChanged(void); + void currentPolygonIndexChanged(int index); + void currentFileChanged(); + void wimaBridgeChanged(); + void synchronizedChanged(void); + void needsUpdateChanged(void); + void readyForSynchronizationChanged(void); + void surveyReadyChanged(void); + void progressLockedChanged(); + +private slots: + void updatePolygonInteractivity(int index); + void _update(); + void CSDestroyedHandler(); + void CSVisualTransectPointsChangedHandler(); + void mAreaPathChangedHandler(); + void mAreaTilesChangedHandler(); + void mAreaProgressChangedHandler(); + void mAreaProgressAcceptedHandler(); + void mAreaReadyChangedHandler(); + void sAreaPathChangedHandler(); + void corridorPathChangedHandler(); + void depotChangedHandler(); + void missionControllerVisualItemsChangedHandler(); + void missionControllerWaypointPathChangedHandler(); + void missionControllerNewItemsFromVehicleHandler(); + void missionControllerMissionItemCountChangedHandler(); + void nemoInterfaceProgressChangedHandler(); + +#ifndef NDEBUG + void autoLoadMission(void); +#endif + +private: +signals: + void joinedAreaValidChanged(); + void stateChanged(); + +private: + // Member Functions + bool toPlanData(AreaData &planData); + bool shortestPath(const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path); + void setSynchronized(bool s); + void enableAreaMonitoring(); + void disableAreaMonitoring(); + void enableMissionControllerMonitoring(); + void disableMissionControllerMonitoring(); + bool areasMonitored(); + bool missionControllerMonitored(); + + // Member Variables + PlanMasterController *_masterController; + MissionController *_missionController; + int _currentAreaIndex; + QString _currentFile; + + WimaMeasurementArea _measurementArea; + WimaServiceArea _serviceArea; + WimaCorridor _corridor; + WimaJoinedArea _joinedArea; + QmlObjectListModel _visualItems; // all areas + + CircularSurvey *_survey; + +#ifndef NDEBUG + QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents + // seg. faults +#endif + + NemoInterface _nemoInterface; + + // State + QScopedPointer _stateMachine; + bool _areasMonitored; + bool _missionControllerMonitored; + bool _progressLocked; + bool _synchronized; // true if planData is synchronized with + // wimaController +}; diff --git a/src/RouteMissionItem/WimaStateMachine.cpp b/src/RouteMissionItem/WimaStateMachine.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3c630f4eab8ab41d74034f0a8b012f2d3e950d2e --- /dev/null +++ b/src/RouteMissionItem/WimaStateMachine.cpp @@ -0,0 +1,380 @@ +#include "WimaStateMachine.h" + +#include "QGCLoggingCategory.h" + +#include + +const QLoggingCategory &WimaPlanerLog(); + +namespace wima_planer_detail { + +template +constexpr typename std::underlying_type::type integral(T value) { + return static_cast::type>(value); +} + +WimaStateMachine::WimaStateMachine(QObject *parent) + : QObject(parent), _state(STATE::NEEDS_INIT) {} + +STATE WimaStateMachine::state() { return this->_state; } + +void WimaStateMachine::updateState(EVENT e) { + qCDebug(WimaPlanerLog) << "StateMachine::updateState(): event:" << e; + switch (this->_state) { + case STATE::NEEDS_INIT: + switch (e) { + case EVENT::INIT_DONE: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_NOT_READY: + case EVENT::M_AREA_READY: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::MISSION_ITEMS_DESTROYED: + case EVENT::SURVEY_UPDATE_TRIGGERED: + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_CHANGED: + case EVENT::PATH_UPDATED: + break; + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + } + break; // STATE::NEEDS_INIT + case STATE::WAITING_FOR_TILE_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_NOT_READY: + break; + case EVENT::M_AREA_READY: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::MISSION_ITEMS_DESTROYED: + case EVENT::SURVEY_UPDATE_TRIGGERED: + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_CHANGED: + case EVENT::PATH_UPDATED: + break; + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + } + break; // STATE::NEEDS_INIT + case STATE::NEEDS_J_AREA_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + break; + case EVENT::M_AREA_NOT_READY: + setState(STATE::WAITING_FOR_TILE_UPDATE); + break; + case EVENT::M_AREA_READY: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + break; + case EVENT::J_AREA_UPDATED: + setState(STATE::NEEDS_SURVEY_UPDATE); + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::MISSION_ITEMS_DESTROYED: + case EVENT::SURVEY_UPDATE_TRIGGERED: + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_CHANGED: + case EVENT::PATH_UPDATED: + break; + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + } + break; // STATE::NEEDS_J_AREA_UPDATE + case STATE::NEEDS_SURVEY_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_NOT_READY: + setState(STATE::WAITING_FOR_TILE_UPDATE); + break; + case EVENT::M_AREA_READY: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::MISSION_ITEMS_DESTROYED: + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + setState(STATE::WAITING_FOR_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_CHANGED: + case EVENT::PATH_UPDATED: + break; + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + } + break; // STATE::NEEDS_SURVEY_UPDATE + case STATE::WAITING_FOR_SURVEY_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_NOT_READY: + setState(STATE::WAITING_FOR_TILE_UPDATE); + break; + case EVENT::M_AREA_READY: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::MISSION_ITEMS_DESTROYED: + setState(STATE::NEEDS_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + break; + case EVENT::SURVEY_UPDATED: + setState(STATE::NEEDS_PATH_UPDATE); + case EVENT::PATH_CHANGED: + case EVENT::PATH_UPDATED: + break; + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + } + break; // STATE::WAYTING_FOR_SURVEY_UPDATE + case STATE::NEEDS_PATH_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_NOT_READY: + setState(STATE::WAITING_FOR_TILE_UPDATE); + break; + case EVENT::M_AREA_READY: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::MISSION_ITEMS_DESTROYED: + setState(STATE::NEEDS_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + setState(STATE::WAITING_FOR_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_CHANGED: + break; + case EVENT::PATH_UPDATED: + setState(STATE::UP_TO_DATE); + break; + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + } + break; // STATE::NEEDS_PATH_UPDATE + case STATE::UP_TO_DATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_NOT_READY: + setState(STATE::WAITING_FOR_TILE_UPDATE); + break; + case EVENT::M_AREA_READY: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::MISSION_ITEMS_DESTROYED: + setState(STATE::NEEDS_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + setState(STATE::WAITING_FOR_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_CHANGED: + setState(STATE::NEEDS_PATH_UPDATE); + break; + case EVENT::PATH_UPDATED: + break; + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + } + break; // STATE::UP_TO_DATE + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown state: " << this->_state; + Q_ASSERT(false); + } +} + +bool WimaStateMachine::upToDate() { return upToDate(this->_state); } + +bool WimaStateMachine::surveyReady() { return surveyReady(this->_state); } + +void WimaStateMachine::setState(STATE s) { + if (this->_state != s) { + auto oldState = this->_state; + this->_state = s; + emit stateChanged(); + if (upToDate(oldState) != upToDate(s)) { + emit upToDateChanged(); + } + if (surveyReady(oldState) != surveyReady(s)) { + emit surveyReadyChanged(); + } + qCDebug(WimaPlanerLog) << "StateMachine::setState():" << oldState << "->" + << s; + } +} + +bool WimaStateMachine::surveyReady(STATE s) { + // Using a switch to enable compiler checking of used states. + bool value = false; + switch (s) { + case STATE::NEEDS_INIT: + case STATE::WAITING_FOR_TILE_UPDATE: + case STATE::NEEDS_J_AREA_UPDATE: + case STATE::NEEDS_SURVEY_UPDATE: + case STATE::WAITING_FOR_SURVEY_UPDATE: + break; + case STATE::NEEDS_PATH_UPDATE: + case STATE::UP_TO_DATE: + value = true; + break; + } + return value; +} + +bool WimaStateMachine::upToDate(STATE s) { + // Using a switch to enable compiler checking of used states. + bool value = false; + switch (s) { + case STATE::NEEDS_INIT: + case STATE::WAITING_FOR_TILE_UPDATE: + case STATE::NEEDS_J_AREA_UPDATE: + case STATE::NEEDS_SURVEY_UPDATE: + case STATE::WAITING_FOR_SURVEY_UPDATE: + case STATE::NEEDS_PATH_UPDATE: + break; + case STATE::UP_TO_DATE: + value = true; + break; + } + return value; +} + +QDebug &operator<<(QDebug &ds, STATE s) { + switch (s) { + case STATE::NEEDS_INIT: + ds << "NEEDS_INIT"; + break; + case STATE::WAITING_FOR_TILE_UPDATE: + ds << "WAITING_FOR_TILE_UPDATE"; + break; + case STATE::NEEDS_J_AREA_UPDATE: + ds << "NEEDS_J_AREA_UPDATE"; + break; + case STATE::NEEDS_SURVEY_UPDATE: + ds << "NEEDS_SURVEY_UPDATE"; + break; + case STATE::WAITING_FOR_SURVEY_UPDATE: + ds << "WAITING_FOR_SURVEY_UPDATE"; + break; + case STATE::NEEDS_PATH_UPDATE: + ds << "NEEDS_PATH_UPDATE"; + break; + case STATE::UP_TO_DATE: + ds << "UP_TO_DATE"; + break; + } + return ds; +} + +QDebug &operator<<(QDebug &ds, EVENT s) { + switch (s) { + case EVENT::INIT_DONE: + ds << "INIT_DONE"; + break; + case EVENT::M_AREA_NOT_READY: + ds << "M_AREA_NOT_READY"; + break; + case EVENT::M_AREA_READY: + ds << "M_AREA_READY"; + break; + case EVENT::M_AREA_PATH_CHANGED: + ds << "M_AREA_PATH_CHANGED"; + break; + case EVENT::S_AREA_PATH_CHANGED: + ds << "S_AREA_PATH_CHANGED"; + break; + case EVENT::CORRIDOR_PATH_CHANGED: + ds << "CORRIDOR_PATH_CHANGED"; + break; + case EVENT::M_AREA_TILES_CHANGED: + ds << "M_AREA_TILES_CHANGED"; + break; + case EVENT::M_AREA_PROGRESS_CHANGED: + ds << "M_AREA_PROGRESS_CHANGED"; + break; + case EVENT::J_AREA_UPDATED: + ds << "J_AREA_UPDATED"; + break; + case EVENT::DEPOT_CHANGED: + ds << "DEPOT_CHANGED"; + break; + case EVENT::SURVEY_DESTROYED: + ds << "SURVEY_DESTROYED"; + break; + case EVENT::MISSION_ITEMS_DESTROYED: + ds << "MISSION_ITEMS_DESTROYED"; + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + ds << "SURVEY_UPDATE_TRIGGERED"; + break; + case EVENT::SURVEY_UPDATED: + ds << "SURVEY_UPDATED"; + break; + case EVENT::PATH_CHANGED: + ds << "PATH_CHANGED"; + break; + case EVENT::PATH_UPDATED: + ds << "PATH_UPDATED"; + break; + } + return ds; +} + +} // namespace wima_planer_detail diff --git a/src/RouteMissionItem/WimaStateMachine.h b/src/RouteMissionItem/WimaStateMachine.h new file mode 100644 index 0000000000000000000000000000000000000000..884d0666c68c0117679426cec5cf126b9a302403 --- /dev/null +++ b/src/RouteMissionItem/WimaStateMachine.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include + +namespace wima_planer_detail { + +enum class STATE { + NEEDS_INIT, + WAITING_FOR_TILE_UPDATE, + NEEDS_J_AREA_UPDATE, + NEEDS_SURVEY_UPDATE, + WAITING_FOR_SURVEY_UPDATE, + NEEDS_PATH_UPDATE, + UP_TO_DATE +}; + +QDebug &operator<<(QDebug &ds, STATE s); + +enum class EVENT { + INIT_DONE, + M_AREA_NOT_READY, + M_AREA_READY, + M_AREA_PATH_CHANGED, + S_AREA_PATH_CHANGED, + CORRIDOR_PATH_CHANGED, + M_AREA_TILES_CHANGED, + M_AREA_PROGRESS_CHANGED, + J_AREA_UPDATED, + DEPOT_CHANGED, + SURVEY_DESTROYED, + MISSION_ITEMS_DESTROYED, + SURVEY_UPDATE_TRIGGERED, + SURVEY_UPDATED, + PATH_CHANGED, + PATH_UPDATED +}; + +QDebug &operator<<(QDebug &ds, EVENT s); + +class WimaStateMachine : public QObject { + Q_OBJECT +public: + explicit WimaStateMachine(QObject *parent = nullptr); + + STATE state(); + void updateState(EVENT e); + bool upToDate(); + bool surveyReady(); + +signals: + void stateChanged(); + void upToDateChanged(); + void surveyReadyChanged(); + +private: + void setState(STATE s); + bool surveyReady(STATE s); + bool upToDate(STATE s); + + STATE _state; +}; + +} // namespace wima_planer_detail diff --git a/src/RouteMissionItem/call_once.h b/src/RouteMissionItem/call_once.h new file mode 100644 index 0000000000000000000000000000000000000000..966f2afa256a8183b56f44dab5ec163e633b245b --- /dev/null +++ b/src/RouteMissionItem/call_once.h @@ -0,0 +1,48 @@ +#ifndef CALL_ONCE_H +#define CALL_ONCE_H + +#include +#include +#include +#include +#include +#include + +namespace CallOnce { +enum ECallOnce { CO_Request, CO_InProgress, CO_Finished }; + +Q_GLOBAL_STATIC(QThreadStorage, once_flag) +} // namespace CallOnce + +template +inline static void qCallOnce(Function func, QBasicAtomicInt &flag) { + using namespace CallOnce; + +#if QT_VERSION < 0x050000 + int protectFlag = flag.fetchAndStoreAcquire(flag); +#elif QT_VERSION >= 0x050000 + int protectFlag = flag.fetchAndStoreAcquire(flag.load()); +#endif + + if (protectFlag == CO_Finished) + return; + if (protectFlag == CO_Request && + flag.testAndSetRelaxed(protectFlag, CO_InProgress)) { + func(); + flag.fetchAndStoreRelease(CO_Finished); + } else { + do { + QThread::yieldCurrentThread(); + } while (!flag.testAndSetAcquire(CO_Finished, CO_Finished)); + } +} + +template inline static void qCallOncePerThread(Function func) { + using namespace CallOnce; + if (!once_flag()->hasLocalData()) { + once_flag()->setLocalData(new QAtomicInt(CO_Request)); + qCallOnce(func, *once_flag()->localData()); + } +} + +#endif // CALL_ONCE_H diff --git a/src/RouteMissionItem/geometry/GenericCircle.h b/src/RouteMissionItem/geometry/GenericCircle.h new file mode 100644 index 0000000000000000000000000000000000000000..8d35c785ac0baf8615518e501e7d115bf1de70ff --- /dev/null +++ b/src/RouteMissionItem/geometry/GenericCircle.h @@ -0,0 +1,154 @@ +#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/RouteMissionItem/geometry/GenericPolygon.h b/src/RouteMissionItem/geometry/GenericPolygon.h new file mode 100644 index 0000000000000000000000000000000000000000..974f49da6d504627f3f60b04bfe7f057643b6cee --- /dev/null +++ b/src/RouteMissionItem/geometry/GenericPolygon.h @@ -0,0 +1,29 @@ +#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/RouteMissionItem/geometry/GenericPolygonArray.h b/src/RouteMissionItem/geometry/GenericPolygonArray.h new file mode 100644 index 0000000000000000000000000000000000000000..68dfa14ae0afa5490df03e7fb57d348919347a12 --- /dev/null +++ b/src/RouteMissionItem/geometry/GenericPolygonArray.h @@ -0,0 +1,58 @@ +#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/RouteMissionItem/geometry/GeoPoint3D.cpp b/src/RouteMissionItem/geometry/GeoPoint3D.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2d5bd3d59366fa4e1c764f943e148dda65197651 --- /dev/null +++ b/src/RouteMissionItem/geometry/GeoPoint3D.cpp @@ -0,0 +1,37 @@ +#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/RouteMissionItem/geometry/GeoPoint3D.h b/src/RouteMissionItem/geometry/GeoPoint3D.h new file mode 100644 index 0000000000000000000000000000000000000000..63b221d117e52c061d27e3f48bcea27e3cc7f3f4 --- /dev/null +++ b/src/RouteMissionItem/geometry/GeoPoint3D.h @@ -0,0 +1,32 @@ +#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/RouteMissionItem/geometry/PlanimetryCalculus.cc b/src/RouteMissionItem/geometry/PlanimetryCalculus.cc new file mode 100644 index 0000000000000000000000000000000000000000..d8053d351c3d131cdfb41c5c7ca6de26cd0a5d8e --- /dev/null +++ b/src/RouteMissionItem/geometry/PlanimetryCalculus.cc @@ -0,0 +1,774 @@ +#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/RouteMissionItem/geometry/PlanimetryCalculus.h b/src/RouteMissionItem/geometry/PlanimetryCalculus.h new file mode 100644 index 0000000000000000000000000000000000000000..c54946dc5cc2fad09f3d6f205c534da80caa075c --- /dev/null +++ b/src/RouteMissionItem/geometry/PlanimetryCalculus.h @@ -0,0 +1,124 @@ +#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/RouteMissionItem/geometry/PolygonCalculus.cc b/src/RouteMissionItem/geometry/PolygonCalculus.cc new file mode 100644 index 0000000000000000000000000000000000000000..a296ed1e98b74e23bef029f011e279f5e05d30ed --- /dev/null +++ b/src/RouteMissionItem/geometry/PolygonCalculus.cc @@ -0,0 +1,578 @@ +#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/RouteMissionItem/geometry/PolygonCalculus.h b/src/RouteMissionItem/geometry/PolygonCalculus.h new file mode 100644 index 0000000000000000000000000000000000000000..6914a1555c855f5e5aebe120d89bda1f68e784a6 --- /dev/null +++ b/src/RouteMissionItem/geometry/PolygonCalculus.h @@ -0,0 +1,45 @@ +#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/RouteMissionItem/geometry/WimaArea.cc b/src/RouteMissionItem/geometry/WimaArea.cc new file mode 100644 index 0000000000000000000000000000000000000000..f83a0d79f8e30847d8336548f45b5a3093682a78 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaArea.cc @@ -0,0 +1,525 @@ +#include "WimaArea.h" + +const char *WimaArea::wimaAreaName = "WimaArea"; +const char *WimaArea::areaTypeName = "AreaType"; +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/RouteMissionItem/geometry/WimaArea.h b/src/RouteMissionItem/geometry/WimaArea.h new file mode 100644 index 0000000000000000000000000000000000000000..34b12c51456dbf87932dba7a5fc6f857b4c8a789 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaArea.h @@ -0,0 +1,40 @@ +#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(QString mapVisualQML READ mapVisualQML CONSTANT) + Q_PROPERTY(QString editorQML READ editorQML CONSTANT) + + virtual QString mapVisualQML(void) const = 0; + virtual QString editorQML(void) const = 0; + + void saveToJson(QJsonObject &jsonObject); + bool loadFromJson(const QJsonObject &jsonObject, QString &errorString); + + // static Members + static const char *wimaAreaName; + static const char *areaTypeName; + static const char *settingsGroup; + +private: + void init(); +}; diff --git a/src/RouteMissionItem/geometry/WimaAreaData.cc b/src/RouteMissionItem/geometry/WimaAreaData.cc new file mode 100644 index 0000000000000000000000000000000000000000..69756dac197e9d7fe8926fe564dd580352a28ab2 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaAreaData.cc @@ -0,0 +1,127 @@ +#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); +} diff --git a/src/RouteMissionItem/geometry/WimaAreaData.h b/src/RouteMissionItem/geometry/WimaAreaData.h new file mode 100644 index 0000000000000000000000000000000000000000..b9348aa5fcfe9ad63271cc7aea27a023665f7101 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaAreaData.h @@ -0,0 +1,62 @@ +#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/RouteMissionItem/geometry/WimaMeasurementArea.cc b/src/RouteMissionItem/geometry/WimaMeasurementArea.cc new file mode 100644 index 0000000000000000000000000000000000000000..ccc2813964cf465358457deab238fddfb39de5d8 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaMeasurementArea.cc @@ -0,0 +1,396 @@ +#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(); + } + } +} diff --git a/src/RouteMissionItem/geometry/WimaMeasurementArea.h b/src/RouteMissionItem/geometry/WimaMeasurementArea.h new file mode 100644 index 0000000000000000000000000000000000000000..07266032584d19d5dfcdf81560e8bc77e4152fe2 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaMeasurementArea.h @@ -0,0 +1,116 @@ +#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/RouteMissionItem/geometry/WimaMeasurementAreaData.cc b/src/RouteMissionItem/geometry/WimaMeasurementAreaData.cc new file mode 100644 index 0000000000000000000000000000000000000000..5d0b68fcc3b16aaf18b248600ab6404002e08378 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaMeasurementAreaData.cc @@ -0,0 +1,137 @@ +#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/RouteMissionItem/geometry/WimaMeasurementAreaData.h b/src/RouteMissionItem/geometry/WimaMeasurementAreaData.h new file mode 100644 index 0000000000000000000000000000000000000000..dde208291fdd5c69080d651e9f828dcdd3852bf7 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaMeasurementAreaData.h @@ -0,0 +1,65 @@ +#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/RouteMissionItem/geometry/WimaServiceArea.cc b/src/RouteMissionItem/geometry/WimaServiceArea.cc new file mode 100644 index 0000000000000000000000000000000000000000..84b6c6afd10848675321f2039f506eb6d1b5deb3 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaServiceArea.cc @@ -0,0 +1,119 @@ +#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/RouteMissionItem/geometry/WimaServiceArea.h b/src/RouteMissionItem/geometry/WimaServiceArea.h new file mode 100644 index 0000000000000000000000000000000000000000..08ff94c34ccec0a73ebc2f8b09228892b5e9b6d9 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaServiceArea.h @@ -0,0 +1,46 @@ +#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/RouteMissionItem/geometry/WimaServiceAreaData.cc b/src/RouteMissionItem/geometry/WimaServiceAreaData.cc new file mode 100644 index 0000000000000000000000000000000000000000..b90acabfaf55cb705ab665f7368fbc99090a9a1b --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaServiceAreaData.cc @@ -0,0 +1,54 @@ +#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); + } +} diff --git a/src/RouteMissionItem/geometry/WimaServiceAreaData.h b/src/RouteMissionItem/geometry/WimaServiceAreaData.h new file mode 100644 index 0000000000000000000000000000000000000000..2b93a20964008977061f992314174feee299d408 --- /dev/null +++ b/src/RouteMissionItem/geometry/WimaServiceAreaData.h @@ -0,0 +1,37 @@ +#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/RouteMissionItem/geometry/clipper/clipper.cpp b/src/RouteMissionItem/geometry/clipper/clipper.cpp new file mode 100644 index 0000000000000000000000000000000000000000..65affb876d8a57d9a2d775d10d62559435cc3dce --- /dev/null +++ b/src/RouteMissionItem/geometry/clipper/clipper.cpp @@ -0,0 +1,4629 @@ +/******************************************************************************* +* * +* 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/RouteMissionItem/geometry/clipper/clipper.hpp b/src/RouteMissionItem/geometry/clipper/clipper.hpp new file mode 100644 index 0000000000000000000000000000000000000000..33043fdb647578aef71adc99ba16091d3a182555 --- /dev/null +++ b/src/RouteMissionItem/geometry/clipper/clipper.hpp @@ -0,0 +1,406 @@ +/******************************************************************************* +* * +* 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/RouteMissionItem/geometry/json/WimaArea.SettingsGroup.json b/src/RouteMissionItem/geometry/json/WimaArea.SettingsGroup.json new file mode 100644 index 0000000000000000000000000000000000000000..321fa4747e2e973f30e8d0831bbd879620119e7a --- /dev/null +++ b/src/RouteMissionItem/geometry/json/WimaArea.SettingsGroup.json @@ -0,0 +1,17 @@ +[ +{ + "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/RouteMissionItem/geometry/json/WimaMeasurementArea.SettingsGroup.json b/src/RouteMissionItem/geometry/json/WimaMeasurementArea.SettingsGroup.json new file mode 100644 index 0000000000000000000000000000000000000000..c4fda54e2296fc8eaf992feaac5054d0aa969c90 --- /dev/null +++ b/src/RouteMissionItem/geometry/json/WimaMeasurementArea.SettingsGroup.json @@ -0,0 +1,45 @@ +[ +{ + "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/RouteMissionItem/geometry/mapbox/feature.hpp b/src/RouteMissionItem/geometry/mapbox/feature.hpp new file mode 100644 index 0000000000000000000000000000000000000000..8d03f4674c3fb71f15e436561f06e454e71c4b46 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/feature.hpp @@ -0,0 +1,113 @@ +#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/RouteMissionItem/geometry/mapbox/geometry.hpp b/src/RouteMissionItem/geometry/mapbox/geometry.hpp new file mode 100644 index 0000000000000000000000000000000000000000..9caecd9a108c6b452f279c6649519977dd787ac1 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include diff --git a/src/RouteMissionItem/geometry/mapbox/geometry/box.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/box.hpp new file mode 100644 index 0000000000000000000000000000000000000000..bb3ea5701423601db2a0c4d37b11b7378589a0ea --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/box.hpp @@ -0,0 +1,36 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/empty.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/empty.hpp new file mode 100644 index 0000000000000000000000000000000000000000..0ea446d8bc76dcca73f57563c41426fde6ba5c34 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/empty.hpp @@ -0,0 +1,18 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/envelope.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/envelope.hpp new file mode 100644 index 0000000000000000000000000000000000000000..e1f722fc540973322731bf73b80145a6950b5ebd --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/envelope.hpp @@ -0,0 +1,33 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/for_each_point.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/for_each_point.hpp new file mode 100644 index 0000000000000000000000000000000000000000..d31b484ffd708c868b51dd1bc45f9e7d73b91158 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/for_each_point.hpp @@ -0,0 +1,51 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/geometry.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/geometry.hpp new file mode 100644 index 0000000000000000000000000000000000000000..fa1e7c31c4cf3cc2443d586a5422d3758ab4d63a --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/geometry.hpp @@ -0,0 +1,56 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/line_string.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/line_string.hpp new file mode 100644 index 0000000000000000000000000000000000000000..a015f71d1cf96c73e4f69dbcf5ea115c3f6fc50f --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/line_string.hpp @@ -0,0 +1,28 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/multi_line_string.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/multi_line_string.hpp new file mode 100644 index 0000000000000000000000000000000000000000..b528fa61be3bd963546b0f43d4c4f0f8f70668e0 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/multi_line_string.hpp @@ -0,0 +1,28 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/multi_point.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/multi_point.hpp new file mode 100644 index 0000000000000000000000000000000000000000..060927bf2308f94b8dcd55442867769f92b90937 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/multi_point.hpp @@ -0,0 +1,28 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/multi_polygon.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/multi_polygon.hpp new file mode 100644 index 0000000000000000000000000000000000000000..9546860ede4a368d5e2922a3825e80e1110130dd --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/multi_polygon.hpp @@ -0,0 +1,28 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/point.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/point.hpp new file mode 100644 index 0000000000000000000000000000000000000000..da8d67733f2a51708d5516d8fc4d52a63bacd3ef --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/point.hpp @@ -0,0 +1,42 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/point_arithmetic.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/point_arithmetic.hpp new file mode 100644 index 0000000000000000000000000000000000000000..0c4c63278a58c07f605cdcda7134e87201e931aa --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/point_arithmetic.hpp @@ -0,0 +1,119 @@ +#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/RouteMissionItem/geometry/mapbox/geometry/polygon.hpp b/src/RouteMissionItem/geometry/mapbox/geometry/polygon.hpp new file mode 100644 index 0000000000000000000000000000000000000000..5cad7bbf5580e476c1d2c2d9f524f24e68bf66a4 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry/polygon.hpp @@ -0,0 +1,45 @@ +#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/RouteMissionItem/geometry/mapbox/geometry_io.hpp b/src/RouteMissionItem/geometry/mapbox/geometry_io.hpp new file mode 100644 index 0000000000000000000000000000000000000000..4d91ccc961a79cfd303cac3b5c52230a448a9933 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/geometry_io.hpp @@ -0,0 +1,98 @@ +#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/RouteMissionItem/geometry/mapbox/optional.hpp b/src/RouteMissionItem/geometry/mapbox/optional.hpp new file mode 100644 index 0000000000000000000000000000000000000000..1185894e7363497e7529a0eb8abb51e6d09e38a7 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/optional.hpp @@ -0,0 +1,74 @@ +#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/RouteMissionItem/geometry/mapbox/polylabel.hpp b/src/RouteMissionItem/geometry/mapbox/polylabel.hpp new file mode 100644 index 0000000000000000000000000000000000000000..0e9e2b5433a311c72a6182125890c0f09ca39742 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/polylabel.hpp @@ -0,0 +1,178 @@ +#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/RouteMissionItem/geometry/mapbox/recursive_wrapper.hpp b/src/RouteMissionItem/geometry/mapbox/recursive_wrapper.hpp new file mode 100644 index 0000000000000000000000000000000000000000..4becdd689d46649987c7779717ff4965752698b1 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/recursive_wrapper.hpp @@ -0,0 +1,122 @@ +#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/RouteMissionItem/geometry/mapbox/variant.hpp b/src/RouteMissionItem/geometry/mapbox/variant.hpp new file mode 100644 index 0000000000000000000000000000000000000000..db5d3c86b58caf3a787e9b8e8d3fc7d8e5042fa9 --- /dev/null +++ b/src/RouteMissionItem/geometry/mapbox/variant.hpp @@ -0,0 +1,901 @@ +#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