diff --git a/QGCCommon.pri b/QGCCommon.pri index 1e1f118c897006c3e9a4ec0ac412d62da910c4f5..6e9f6cd15de8a4f99c417cd463cc72f65e75dfce 100644 --- a/QGCCommon.pri +++ b/QGCCommon.pri @@ -35,6 +35,8 @@ linux { QMAKE_CXXFLAGS_WARN_ON += -Werror \ -Wno-deprecated-copy \ # These come from mavlink headers -Wno-unused-parameter \ # gst_plugins-good has these errors + -Wno-ignored-qualifiers\ # or-tools has these errors + -Wno-sign-compare\ # or-tools has these errors -Wno-implicit-fallthrough # gst_plugins-good has these errors } } else : linux-rasp-pi2-g++ { diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d3ab5f2bcec3ee96d87d4cfbd1335278bcb0d349..d5fd5f1924476e4b0c02c2abd741ad3f9f526954 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -445,6 +445,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) { # HEADERS += \ + src/MeasurementComplexItem/geometry/geometry.h \ src/QmlControls/QmlUnitsConversion.h \ src/MeasurementComplexItem/geometry/GeoArea.h \ src/MeasurementComplexItem/geometry/MeasurementArea.h \ @@ -479,7 +480,6 @@ HEADERS += \ src/MeasurementComplexItem/geometry/mapbox/recursive_wrapper.hpp \ src/MeasurementComplexItem/geometry/mapbox/variant.hpp \ src/MeasurementComplexItem/geometry/mapbox/variant_io.hpp \ - src/MeasurementComplexItem/geometry/snake.h \ src/MeasurementComplexItem/geometry/GenericPolygon.h \ src/MeasurementComplexItem/geometry/GenericPolygonArray.h \ src/MeasurementComplexItem/geometry/GeoPoint3D.h \ @@ -524,6 +524,7 @@ SOURCES += \ src/MeasurementComplexItem/geometry/GeoArea.cc \ src/MeasurementComplexItem/geometry/MeasurementArea.cc \ src/MeasurementComplexItem/geometry/SafeArea.cc \ + src/MeasurementComplexItem/geometry/geometry.cpp \ src/Vehicle/VehicleEscStatusFactGroup.cc \ src/MeasurementComplexItem/AreaData.cc \ src/api/QGCCorePlugin.cc \ @@ -537,7 +538,6 @@ SOURCES += \ src/MeasurementComplexItem/GeneratorBase.cc \ src/MeasurementComplexItem/LinearGenerator.cpp \ src/MeasurementComplexItem/geometry/clipper/clipper.cpp \ - src/MeasurementComplexItem/geometry/snake.cpp \ src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \ src/MeasurementComplexItem/NemoInterface.cpp \ src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc \ diff --git a/src/MeasurementComplexItem/AreaData.cc b/src/MeasurementComplexItem/AreaData.cc index 165e531a5de0069d9f267fe4e9f49b532f1f6e74..48aafe9e908f8cac3771d43845e29822ccf89a2e 100644 --- a/src/MeasurementComplexItem/AreaData.cc +++ b/src/MeasurementComplexItem/AreaData.cc @@ -2,7 +2,7 @@ #include "geometry/MeasurementArea.h" #include "geometry/SafeArea.h" -#include "geometry/snake.h" +#include "geometry/geometry.h" #include "JsonHelper.h" #include "QGCApplication.h" @@ -116,10 +116,10 @@ bool AreaData::isCorrect(bool showError) { return false; } const auto &origin = this->origin(); - snake::FPolygon safeAreaENU; - snake::areaToEnu(origin, safeArea->pathModel(), safeAreaENU); - snake::FPolygon measurementAreaENU; - snake::areaToEnu(origin, measurementArea->pathModel(), measurementAreaENU); + geometry::FPolygon safeAreaENU; + geometry::areaToEnu(origin, safeArea->pathModel(), safeAreaENU); + geometry::FPolygon measurementAreaENU; + geometry::areaToEnu(origin, measurementArea->pathModel(), measurementAreaENU); // qDebug() << "origin" << origin; // std::stringstream ss; // ss << "measurementAreaENU: " << bg::wkt(measurementAreaENU) << std::endl; @@ -218,14 +218,14 @@ void AreaData::intersection(bool showError) { // convert to ENU const auto origin = this->origin(); - snake::FPolygon safeAreaENU; - snake::areaToEnu(origin, safeArea->pathModel(), safeAreaENU); - snake::FPolygon measurementAreaENU; - snake::areaToEnu(origin, measurementArea->pathModel(), + geometry::FPolygon safeAreaENU; + geometry::areaToEnu(origin, safeArea->pathModel(), safeAreaENU); + geometry::FPolygon measurementAreaENU; + geometry::areaToEnu(origin, measurementArea->pathModel(), measurementAreaENU); // do intersection - std::deque outputENU; + std::deque outputENU; boost::geometry::intersection(measurementAreaENU, safeAreaENU, outputENU); if (outputENU.size() < 1 || outputENU[0].outer().size() < 4) { @@ -246,9 +246,9 @@ void AreaData::intersection(bool showError) { // Shrink the result if safeAreaENU doesn't cover it. auto large = std::move(outputENU[0]); - snake::FPolygon small; + geometry::FPolygon small; while (!bg::covered_by(large, safeAreaENU)) { - snake::offsetPolygon(large, small, -0.1); + geometry::offsetPolygon(large, small, -0.1); large = std::move(small); } @@ -259,7 +259,7 @@ void AreaData::intersection(bool showError) { for (auto it = large.outer().begin(); it != large.outer().end() - 1; ++it) { QGeoCoordinate c; - snake::fromENU(origin, *it, c); + geometry::fromENU(origin, *it, c); measurementArea->appendVertex(c); } } diff --git a/src/MeasurementComplexItem/CircularGenerator.cpp b/src/MeasurementComplexItem/CircularGenerator.cpp index 91642291519da2ab333452e64cb87e1ade99a4b2..d8c7ffa1523d9ead0f76e383ec9ecd8f2a07aa4a 100644 --- a/src/MeasurementComplexItem/CircularGenerator.cpp +++ b/src/MeasurementComplexItem/CircularGenerator.cpp @@ -31,11 +31,12 @@ const char *minLengthKey = "MinLength"; const char *referenceKey = "ReferencePoint"; } // namespace -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); +bool circularTransects(const geometry::FPoint &reference, + const geometry::FPolygon &polygon, + const std::vector &tiles, + geometry::Length deltaR, geometry::Angle deltaAlpha, + geometry::Length minLength, + geometry::LineStringArray &transects); const char *CircularGenerator::settingsGroup = "CircularGenerator"; const char *CircularGenerator::typeString = "CircularGenerator"; @@ -76,7 +77,7 @@ QString CircularGenerator::abbreviation() const { return tr("C. Gen."); } QString CircularGenerator::type() const { return typeString; } -bool CircularGenerator::get(Generator &generator) { +bool CircularGenerator::get(Work &work) { if (this->_d) { if (this->_d->isCorrect()) { // Prepare data. @@ -93,8 +94,8 @@ bool CircularGenerator::get(Generator &generator) { qCDebug(CircularGeneratorLog) << "get(): reference invalid." << ref; return false; } - snake::FPoint reference; - snake::toENU(origin, ref, reference); + geometry::FPoint reference; + geometry::toENU(origin, ref, reference); auto measurementArea = getGeoArea(*this->_d->areaList()); @@ -114,13 +115,13 @@ bool CircularGenerator::get(Generator &generator) { return false; } } - auto pPolygon = std::make_shared(); - snake::areaToEnu(origin, geoPolygon, *pPolygon); + auto pPolygon = std::make_shared(); + geometry::areaToEnu(origin, geoPolygon, *pPolygon); // Progress and tiles. const auto &progress = measurementArea->progress(); const auto *tiles = measurementArea->tiles(); - auto pTiles = std::make_shared>(); + auto pTiles = std::make_shared>(); if (progress.size() == tiles->count()) { for (int i = 0; i < tiles->count(); ++i) { if (progress[i] == 100) { @@ -128,8 +129,8 @@ bool CircularGenerator::get(Generator &generator) { const auto *tile = qobject_cast(obj); if (tile != nullptr) { - snake::FPolygon tileENU; - snake::areaToEnu(origin, tile->coordinateList(), tileENU); + geometry::FPolygon tileENU; + geometry::areaToEnu(origin, tile->coordinateList(), tileENU); pTiles->push_back(std::move(tileENU)); } else { qCDebug(CircularGeneratorLog) @@ -154,22 +155,22 @@ bool CircularGenerator::get(Generator &generator) { qCDebug(CircularGeneratorLog) << "get(): depot invalid." << geoDepot; return false; } - snake::FPoint depot; - snake::toENU(origin, geoDepot, depot); + geometry::FPoint depot; + geometry::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 { + auto distance = geometry::Length(this->_distance.rawValue().toDouble() * + bu::si::meter); + auto minLength = geometry::Length(this->_minLength.rawValue().toDouble() * + bu::si::meter); + auto alpha = geometry::Angle(this->_deltaAlpha.rawValue().toDouble() * + bu::degree::degree); + + work = [reference, depot, pPolygon, pTiles, distance, alpha, + minLength](geometry::LineStringArray &transects) -> bool { bool value = circularTransects(reference, *pPolygon, *pTiles, distance, alpha, minLength, transects); - transects.insert(transects.begin(), snake::FLineString{depot}); + transects.insert(transects.begin(), geometry::FLineString{depot}); return value; }; return true; @@ -413,11 +414,12 @@ void CircularGenerator::setMeasurementArea(MeasurementArea *area) { } } -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) { +bool circularTransects(const geometry::FPoint &reference, + const geometry::FPolygon &polygon, + const std::vector &tiles, + geometry::Length deltaR, geometry::Angle deltaAlpha, + geometry::Length minLength, + geometry::LineStringArray &transects) { auto s1 = std::chrono::high_resolution_clock::now(); // Check preconitions @@ -435,15 +437,16 @@ bool circularTransects(const snake::FPoint &reference, qCDebug(CircularGeneratorLog) << ss.str().c_str(); } else { // Calculate polygon distances and angles. - std::vector distances; + std::vector distances; distances.reserve(polygon.outer().size()); - std::vector angles; + std::vector angles; angles.reserve(polygon.outer().size()); // qCDebug(CircularGeneratorLog) << "circularTransects():"; for (const auto &p : polygon.outer()) { - snake::Length distance = bg::distance(reference, p) * si::meter; + geometry::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; + geometry::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); // qCDebug(CircularGeneratorLog) << "distances, angles, @@ -456,8 +459,8 @@ bool circularTransects(const snake::FPoint &reference, } auto rMin = deltaR; // minimal circle radius - snake::Angle alpha1(0 * degree::degree); - snake::Angle alpha2(360 * degree::degree); + geometry::Angle alpha1(0 * degree::degree); + geometry::Angle alpha2(360 * degree::degree); // Determine r_min by successive approximation if (!bg::within(reference, polygon.outer())) { rMin = bg::distance(reference, polygon) * si::meter; @@ -478,7 +481,7 @@ bool circularTransects(const snake::FPoint &reference, // Generate circle sectors. auto rScaled = rMinScaled; const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value())); - vector sectors(nTran, ClipperLib::Path()); + std::vector sectors(nTran, ClipperLib::Path()); const auto nSectors = long(std::round(((alpha2 - alpha1) / deltaAlpha).value())); // qCDebug(CircularGeneratorLog) << "circularTransects(): sector @@ -504,8 +507,8 @@ bool circularTransects(const snake::FPoint &reference, } // Clip sectors to polygonENU. ClipperLib::Path polygonClipper; - snake::FPolygon shrinked; - snake::offsetPolygon(polygon, shrinked, -0.3); + geometry::FPolygon shrinked; + geometry::offsetPolygon(polygon, shrinked, -0.3); auto &outer = shrinked.outer(); polygonClipper.reserve(outer.size()); for (auto it = outer.begin(); it < outer.end() - 1; ++it) { @@ -522,7 +525,7 @@ bool circularTransects(const snake::FPoint &reference, // Subtract holes. if (tiles.size() > 0) { - vector processedTiles; + std::vector processedTiles; for (const auto &tile : tiles) { ClipperLib::Path path; for (const auto &v : tile.outer()) { @@ -546,12 +549,12 @@ bool circularTransects(const snake::FPoint &reference, // Extract transects from PolyTree and convert them to // BoostLineString for (const auto &child : transectsClipper.Childs) { - snake::FLineString transect; + geometry::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)); + transect.push_back(geometry::FPoint(x, y)); } transects.push_back(transect); } @@ -562,7 +565,7 @@ bool circularTransects(const snake::FPoint &reference, for (auto iti = ito + 1; iti < transects.end(); ++iti) { auto dist1 = bg::distance(ito->front(), iti->front()); if (dist1 < th) { - snake::FLineString temp; + geometry::FLineString temp; for (auto it = iti->end() - 1; it >= iti->begin(); --it) { temp.push_back(*it); } @@ -573,7 +576,7 @@ bool circularTransects(const snake::FPoint &reference, } auto dist2 = bg::distance(ito->front(), iti->back()); if (dist2 < th) { - snake::FLineString temp; + geometry::FLineString temp; temp.insert(temp.end(), iti->begin(), iti->end()); temp.insert(temp.end(), ito->begin(), ito->end()); *ito = temp; @@ -582,7 +585,7 @@ bool circularTransects(const snake::FPoint &reference, } auto dist3 = bg::distance(ito->back(), iti->front()); if (dist3 < th) { - snake::FLineString temp; + geometry::FLineString temp; temp.insert(temp.end(), ito->begin(), ito->end()); temp.insert(temp.end(), iti->begin(), iti->end()); *ito = temp; @@ -591,7 +594,7 @@ bool circularTransects(const snake::FPoint &reference, } auto dist4 = bg::distance(ito->back(), iti->back()); if (dist4 < th) { - snake::FLineString temp; + geometry::FLineString temp; temp.insert(temp.end(), ito->begin(), ito->end()); for (auto it = iti->end() - 1; it >= iti->begin(); --it) { temp.push_back(*it); diff --git a/src/MeasurementComplexItem/CircularGenerator.h b/src/MeasurementComplexItem/CircularGenerator.h index 49d6736dd01eed42b145221c51f2d935e8981f75..f06a5425bdd84c0374d0d82570bbc395e0689c15 100644 --- a/src/MeasurementComplexItem/CircularGenerator.h +++ b/src/MeasurementComplexItem/CircularGenerator.h @@ -26,7 +26,7 @@ public: virtual QString abbreviation() const override; virtual QString type() const override; - virtual bool get(Generator &generator) override; + virtual bool get(Work &work) override; QGeoCoordinate reference() const; Fact *distance(); diff --git a/src/MeasurementComplexItem/GeneratorBase.h b/src/MeasurementComplexItem/GeneratorBase.h index 4d1f6daf45cbe8dd012d167454f3be6692a7a2ff..b0e04b14d9660673db76a86e5b11f1d87b142f1e 100644 --- a/src/MeasurementComplexItem/GeneratorBase.h +++ b/src/MeasurementComplexItem/GeneratorBase.h @@ -7,7 +7,7 @@ #include #include -#include "geometry/snake.h" +#include "geometry/geometry.h" #include "AreaData.h" @@ -17,7 +17,7 @@ class GeneratorBase : public QObject { Q_OBJECT public: using Data = AreaData *; - using Generator = std::function; + using Work = std::function; explicit GeneratorBase(QObject *parent = nullptr); explicit GeneratorBase(Data d, QObject *parent = nullptr); @@ -39,7 +39,7 @@ public: virtual QString abbreviation() const = 0; virtual QString type() const = 0; - virtual bool get(Generator &generator) = 0; + virtual bool get(Work &work) = 0; Data data() const; void setData(Data d); diff --git a/src/MeasurementComplexItem/LinearGenerator.cpp b/src/MeasurementComplexItem/LinearGenerator.cpp index bc3b6ffff2fe303bc272746f4d1bdf02568e902a..fd9eacc8cfb8e24c85dc59bf747819288385396b 100644 --- a/src/MeasurementComplexItem/LinearGenerator.cpp +++ b/src/MeasurementComplexItem/LinearGenerator.cpp @@ -23,10 +23,11 @@ const char *minLengthKey = "MinLength"; QGC_LOGGING_CATEGORY(LinearGeneratorLog, "LinearGeneratorLog") -bool linearTransects(const snake::FPolygon &polygon, - const std::vector &tiles, - snake::Length distance, snake::Angle angle, - snake::Length minLength, snake::Transects &transects); +bool linearTransects(const geometry::FPolygon &polygon, + const std::vector &tiles, + geometry::Length distance, geometry::Angle angle, + geometry::Length minLength, + geometry::LineStringArray &transects); const char *LinearGenerator::settingsGroup = "LinearGenerator"; const char *LinearGenerator::typeString = "LinearGenerator"; @@ -67,7 +68,7 @@ QString LinearGenerator::abbreviation() const { QString LinearGenerator::type() const { return typeString; } -bool LinearGenerator::get(Generator &generator) { +bool LinearGenerator::get(Work &generator) { if (_d != nullptr) { if (this->_d->isCorrect()) { // Prepare data. @@ -95,13 +96,13 @@ bool LinearGenerator::get(Generator &generator) { return false; } } - auto pPolygon = std::make_shared(); - snake::areaToEnu(origin, geoPolygon, *pPolygon); + auto pPolygon = std::make_shared(); + geometry::areaToEnu(origin, geoPolygon, *pPolygon); // Progress and tiles. const auto &progress = measurementArea->progress(); const auto *tiles = measurementArea->tiles(); - auto pTiles = std::make_shared>(); + auto pTiles = std::make_shared>(); if (progress.size() == tiles->count()) { for (int i = 0; i < tiles->count(); ++i) { if (progress[i] == 100) { @@ -109,8 +110,8 @@ bool LinearGenerator::get(Generator &generator) { const auto *tile = qobject_cast(obj); if (tile != nullptr) { - snake::FPolygon tileENU; - snake::areaToEnu(origin, tile->coordinateList(), tileENU); + geometry::FPolygon tileENU; + geometry::areaToEnu(origin, tile->coordinateList(), tileENU); pTiles->push_back(std::move(tileENU)); } else { qCDebug(LinearGeneratorLog) << "get(): tile == nullptr"; @@ -134,21 +135,21 @@ bool LinearGenerator::get(Generator &generator) { qCDebug(LinearGeneratorLog) << "get(): depot invalid." << geoDepot; return false; } - snake::FPoint depot; - snake::toENU(origin, geoDepot, depot); + geometry::FPoint depot; + geometry::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); + auto distance = geometry::Length(this->_distance.rawValue().toDouble() * + bu::si::meter); + auto minLength = geometry::Length(this->_minLength.rawValue().toDouble() * + bu::si::meter); + auto alpha = geometry::Angle(this->_alpha.rawValue().toDouble() * + bu::degree::degree); generator = [depot, pPolygon, pTiles, distance, alpha, - minLength](snake::Transects &transects) -> bool { + minLength](geometry::LineStringArray &transects) -> bool { bool value = linearTransects(*pPolygon, *pTiles, distance, alpha, minLength, transects); - transects.insert(transects.begin(), snake::FLineString{depot}); + transects.insert(transects.begin(), geometry::FLineString{depot}); return value; }; return true; @@ -326,10 +327,11 @@ void LinearGenerator::setMeasurementArea(MeasurementArea *area) { } } -bool linearTransects(const snake::FPolygon &polygon, - const std::vector &tiles, - snake::Length distance, snake::Angle angle, - snake::Length minLength, snake::Transects &transects) { +bool linearTransects(const geometry::FPolygon &polygon, + const std::vector &tiles, + geometry::Length distance, geometry::Angle angle, + geometry::Length minLength, + geometry::LineStringArray &transects) { namespace tr = bg::strategy::transform; auto s1 = std::chrono::high_resolution_clock::now(); @@ -349,9 +351,9 @@ bool linearTransects(const snake::FPolygon &polygon, tr::rotate_transformer rotate(angle.value() * 180 / M_PI); // Rotate polygon by angle and calculate bounding box. - snake::FPolygon polygonENURotated; + geometry::FPolygon polygonENURotated; bg::transform(polygon.outer(), polygonENURotated.outer(), rotate); - snake::FBox box; + geometry::FBox box; boost::geometry::envelope(polygonENURotated, box); double x0 = box.min_corner().get<0>(); double y0 = box.min_corner().get<1>(); @@ -360,17 +362,17 @@ bool linearTransects(const snake::FPolygon &polygon, // Generate transects and convert them to clipper path. size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects - vector transectsClipper; + std::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; + geometry::FPoint v1{x0, y0 + i * distance.value()}; + geometry::FPoint v2{x1, y0 + i * distance.value()}; + geometry::FLineString transect; transect.push_back(v1); transect.push_back(v2); // transform back - snake::FLineString temp_transect; + geometry::FLineString temp_transect; tr::rotate_transformer rotate_back( -angle.value() * 180 / M_PI); bg::transform(transect, temp_transect, rotate_back); @@ -397,8 +399,8 @@ bool linearTransects(const snake::FPolygon &polygon, } // Convert measurement area to clipper path. - snake::FPolygon shrinked; - snake::offsetPolygon(polygon, shrinked, -0.2); + geometry::FPolygon shrinked; + geometry::offsetPolygon(polygon, shrinked, -0.2); auto &outer = shrinked.outer(); ClipperLib::Path polygonClipper; for (auto vertex : outer) { @@ -418,7 +420,7 @@ bool linearTransects(const snake::FPolygon &polygon, // Subtract holes. if (tiles.size() > 0) { - vector processedTiles; + std::vector processedTiles; for (const auto &tile : tiles) { ClipperLib::Path path; for (const auto &v : tile.outer()) { @@ -442,14 +444,14 @@ bool linearTransects(const snake::FPolygon &polygon, // Extract transects from PolyTree and convert them to BoostLineString for (const auto &child : clippedTransecs.Childs) { const auto &clipperTransect = child->Contour; - snake::FPoint v1{ + geometry::FPoint v1{ static_cast(clipperTransect[0].X) / CLIPPER_SCALE, static_cast(clipperTransect[0].Y) / CLIPPER_SCALE}; - snake::FPoint v2{ + geometry::FPoint v2{ static_cast(clipperTransect[1].X) / CLIPPER_SCALE, static_cast(clipperTransect[1].Y) / CLIPPER_SCALE}; - snake::FLineString transect{v1, v2}; + geometry::FLineString transect{v1, v2}; if (bg::length(transect) >= minLength.value()) { transects.push_back(transect); } diff --git a/src/MeasurementComplexItem/LinearGenerator.h b/src/MeasurementComplexItem/LinearGenerator.h index dc3075da316b1a73ae24d4eaa617939a29a493bc..f22b0fc591a7ca20a75f423acef68acfce562c30 100644 --- a/src/MeasurementComplexItem/LinearGenerator.h +++ b/src/MeasurementComplexItem/LinearGenerator.h @@ -25,7 +25,7 @@ public: virtual QString abbreviation() const override; virtual QString type() const override; - virtual bool get(Generator &generator) override; + virtual bool get(Work &generator) override; //! //! \brief save Saves the generator. diff --git a/src/MeasurementComplexItem/MeasurementComplexItem.cc b/src/MeasurementComplexItem/MeasurementComplexItem.cc index d287a3032a2a56df9cf4c698c545225565af7ccf..867b7cc3931dba594e2f51ea8650282fb1f66f46 100644 --- a/src/MeasurementComplexItem/MeasurementComplexItem.cc +++ b/src/MeasurementComplexItem/MeasurementComplexItem.cc @@ -7,7 +7,7 @@ #include "geometry/MeasurementArea.h" #include "geometry/SafeArea.h" #include "geometry/clipper/clipper.hpp" -#include "geometry/snake.h" +#include "geometry/geometry.h" #include "nemo_interface/SnakeTile.h" // QGC @@ -365,13 +365,13 @@ bool MeasurementComplexItem::load(const QJsonObject &complexObject, auto safeArea = safeAreaArray[0]; QGeoCoordinate origin = safeArea->pathModel().value(0)->coordinate(); - snake::FPolygon safeAreaENU; - snake::areaToEnu(origin, safeArea->coordinateList(), safeAreaENU); + geometry::FPolygon safeAreaENU; + geometry::areaToEnu(origin, safeArea->coordinateList(), safeAreaENU); for (const auto &variant : variantVector) { - snake::FLineString varENU; + geometry::FLineString varENU; for (const auto &vertex : variant) { - snake::FPoint vertexENU; - snake::toENU(origin, vertex.value(), vertexENU); + geometry::FPoint vertexENU; + geometry::toENU(origin, vertex.value(), vertexENU); varENU.push_back(vertexENU); } @@ -833,11 +833,11 @@ void MeasurementComplexItem::_updateRoute() { RoutingParameter par; par.numSolutions = 5; auto &safeAreaENU = par.safeArea; - snake::areaToEnu(origin, geoSafeArea, safeAreaENU); + geometry::areaToEnu(origin, geoSafeArea, safeAreaENU); // Create generator. if (this->_pGenerator != nullptr) { - routing::GeneratorBase::Generator g; // Transect generator. + routing::GeneratorBase::Work g; // Transect generator. if (this->_pGenerator->get(g)) { // Start/Restart routing worker. this->_pWorker->route(par, g); @@ -1182,7 +1182,7 @@ void MeasurementComplexItem::_storeRoutingData( auto ori = this->_pAreaData->origin(); ori.setAltitude(0); QVector variantVector; - const auto nSolutions = pRoute->solutionVector.size(); + const std::size_t nSolutions = pRoute->solutionVector.size(); for (std::size_t j = 0; j < nSolutions; ++j) { Variant var; @@ -1195,7 +1195,7 @@ void MeasurementComplexItem::_storeRoutingData( for (const auto &vertex : path) { QGeoCoordinate c; - snake::fromENU(ori, vertex, c); + geometry::fromENU(ori, vertex, c); var.append(QVariant::fromValue(c)); } } else { diff --git a/src/MeasurementComplexItem/MeasurementComplexItem.h b/src/MeasurementComplexItem/MeasurementComplexItem.h index f87224987f65305b8737deefb4f61cfc1830c127..ab71fa1da1545308b95d17989bff2d8fc9c3edec 100644 --- a/src/MeasurementComplexItem/MeasurementComplexItem.h +++ b/src/MeasurementComplexItem/MeasurementComplexItem.h @@ -12,7 +12,7 @@ #include "AreaData.h" class RoutingThread; -class RoutingData; +class RoutingResult; namespace routing { class GeneratorBase; @@ -23,7 +23,7 @@ class MeasurementComplexItem : public ComplexMissionItem { using PtrGenerator = routing::GeneratorBase *; using PtrAreaData = AreaData *; - using PtrRoutingData = std::shared_ptr; + using PtrRoutingData = std::shared_ptr; using PtrWorker = RoutingThread *; using Variant = QVariantList; diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp index afd765cf4a3d0f40eb8fc03156fc13eea80c0fd3..f8ec2909ff110c46545a9c2135391350d1dee2d9 100644 --- a/src/MeasurementComplexItem/NemoInterface.cpp +++ b/src/MeasurementComplexItem/NemoInterface.cpp @@ -14,7 +14,7 @@ #include "GenericSingelton.h" #include "geometry/MeasurementArea.h" -#include "geometry/snake.h" +#include "geometry/geometry.h" #include "nemo_interface/QNemoHeartbeat.h" #include "nemo_interface/QNemoProgress.h" #include "nemo_interface/SnakeTile.h" @@ -170,7 +170,7 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) { tile = qobject_cast(obj); if (tile != nullptr) { SnakeTileLocal tileENU; - snake::areaToEnu(origin, tile->coordinateList(), tileENU.path()); + geometry::areaToEnu(origin, tile->coordinateList(), tileENU.path()); this->tilesENU.polygons().push_back(std::move(tileENU)); } else { qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr."; diff --git a/src/MeasurementComplexItem/RoutingThread.cpp b/src/MeasurementComplexItem/RoutingThread.cpp index f45bb3c0349231b9e91a4203ef6c38a7ad95a847..630e457eed555132253813eae87b75108a38b1d7 100644 --- a/src/MeasurementComplexItem/RoutingThread.cpp +++ b/src/MeasurementComplexItem/RoutingThread.cpp @@ -3,10 +3,35 @@ #include // Qt #include - +// or-tools #include "QGCLoggingCategory.h" +#include "ortools/constraint_solver/routing.h" +#include "ortools/constraint_solver/routing_enums.pb.h" +#include "ortools/constraint_solver/routing_index_manager.h" +#include "ortools/constraint_solver/routing_parameters.h" + QGC_LOGGING_CATEGORY(RoutingThreadLog, "RoutingThreadLog") +using namespace geometry; +using namespace operations_research; + +// Aux struct and functions. +struct InternalParameters { + InternalParameters() + : numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5), + stop([] { return false; }) {} + std::size_t numSolutionsPerRun; + std::size_t numRuns; + std::size_t minNumTransectsPerRun; + std::function stop; + mutable std::string errorString; +}; + +bool getRoute(const FPolygon &area, const LineStringArray &transects, + std::vector &solutionVector, + const InternalParameters &par = InternalParameters()); + +// class RoutingThread RoutingThread::RoutingThread(QObject *parent) : QThread(parent), _calculating(false), _stop(false), _restart(false) { @@ -26,12 +51,11 @@ RoutingThread::~RoutingThread() { bool RoutingThread::calculating() const { return this->_calculating; } -void RoutingThread::route(const RoutingParameter &par, - const Generator &generator) { +void RoutingThread::route(const RoutingParameter &par, const Work &work) { // Sample input. Lock lk(this->_mutex); this->_par = par; - this->_generator = generator; + this->_work = work; lk.unlock(); if (!this->isRunning()) { @@ -55,16 +79,16 @@ void RoutingThread::run() { emit calculatingChanged(); Lock lk(this->_mutex); auto par = this->_par; - auto generator = this->_generator; + auto work = this->_work; lk.unlock(); auto safeAreaENU = par.safeArea; auto numRuns = par.numRuns; auto numSolutionsPerRun = par.numSolutions; - PtrRoutingData pRouteData(new RoutingData()); + PtrRoutingData pRouteData(new RoutingResult()); auto &transectsENU = pRouteData->transects; // Generate transects. - if (generator(transectsENU)) { + if (work(transectsENU)) { // Check if generation was successful. if (transectsENU.size() == 0) { qCDebug(RoutingThreadLog) << "run(): " @@ -73,29 +97,29 @@ void RoutingThread::run() { // Prepare data for routing. auto &solutionVector = pRouteData->solutionVector; - snake::RouteParameter snakePar; - snakePar.numSolutionsPerRun = numSolutionsPerRun; - snakePar.numRuns = numRuns; + InternalParameters routePar; + routePar.numSolutionsPerRun = numSolutionsPerRun; + routePar.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] { + routePar.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); + getRoute(safeAreaENU, transectsENU, solutionVector, routePar); // Check if routing was successful. if ((!success || solutionVector.size() < 1) && !this->_restart) { qCDebug(RoutingThreadLog) << "run(): " "routing failed. " - << snakePar.errorString.c_str(); + << routePar.errorString.c_str(); } else if (this->_restart) { qCDebug(RoutingThreadLog) << "run(): " "restart requested."; @@ -127,3 +151,402 @@ void RoutingThread::run() { } // main loop qCDebug(RoutingThreadLog) << "run(): thread end."; } + +bool getRoute(const FPolygon &area, const LineStringArray &transects, + std::vector &solutionVector, + const InternalParameters &par) { + +#ifdef SNAKE_SHOW_TIME + auto start = std::chrono::high_resolution_clock::now(); +#endif + //================================================================ + // Create routing model. + //================================================================ + // Use integer polygons to increase numerical robustness. + // Convert area; + IPolygon intArea; + for (const auto &v : area.outer()) { + auto p = float2Int(v); + intArea.outer().push_back(p); + } + for (const auto &ring : area.inners()) { + IRing intRing; + for (const auto &v : ring) { + auto p = float2Int(v); + intRing.push_back(p); + } + intArea.inners().push_back(std::move(intRing)); + } + + // Helper classes. + struct VirtualNode { + VirtualNode(std::size_t f, std::size_t t) : fromIndex(f), toIndex(t) {} + std::size_t fromIndex; // index for leaving node + std::size_t toIndex; // index for entering node + }; + struct NodeToTransect { + NodeToTransect(std::size_t i, bool r) : transectsIndex(i), reversed(r) {} + std::size_t transectsIndex; // transects index + bool reversed; // transect reversed? + }; + // Create vertex and node list + std::vector vertices; + std::vector> disjointNodes; + std::vector nodeList; + std::vector nodeToTransectList; + for (std::size_t i = 0; i < transects.size(); ++i) { + const auto &t = transects[i]; + // Copy line edges only. + if (t.size() == 1 || i == 0) { + auto p = float2Int(t.back()); + vertices.push_back(p); + nodeToTransectList.emplace_back(i, false); + auto idx = vertices.size() - 1; + nodeList.emplace_back(idx, idx); + } else if (t.size() > 1) { + auto p1 = float2Int(t.front()); + auto p2 = float2Int(t.back()); + vertices.push_back(p1); + vertices.push_back(p2); + nodeToTransectList.emplace_back(i, false); + nodeToTransectList.emplace_back(i, true); + auto fromIdx = vertices.size() - 1; + auto toIdx = fromIdx - 1; + nodeList.emplace_back(fromIdx, toIdx); + nodeList.emplace_back(toIdx, fromIdx); + disjointNodes.emplace_back(toIdx, fromIdx); + } else { // transect empty + std::cout << "ignoring empty transect with index " << i << std::endl; + } + } +#ifdef SNAKE_DEBUG + // Print. + std::cout << "nodeToTransectList:" << std::endl; + std::cout << "node:transectIndex:reversed" << std::endl; + std::size_t c = 0; + for (const auto &n2t : nodeToTransectList) { + std::cout << c++ << ":" << n2t.transectsIndex << ":" << n2t.reversed + << std::endl; + } + std::cout << "nodeList:" << std::endl; + std::cout << "node:fromIndex:toIndex" << std::endl; + c = 0; + for (const auto &n : nodeList) { + std::cout << c++ << ":" << n.fromIndex << ":" << n.toIndex << std::endl; + } + std::cout << "disjoint nodes:" << std::endl; + std::cout << "number:nodes" << std::endl; + c = 0; + for (const auto &d : disjointNodes) { + std::cout << c++ << ":" << d.first << "," << d.second << std::endl; + } +#endif + + // Add polygon vertices. + for (auto &v : intArea.outer()) { + vertices.push_back(v); + } + for (auto &ring : intArea.inners()) { + for (auto &v : ring) { + vertices.push_back(v); + } + } + + // Create connection graph (inf == no connection between vertices). + // Note: graph is not symmetric. + auto n = vertices.size(); + // Matrix must be double since integers don't have infinity and nan + Matrix connectionGraph(n, n); + for (std::size_t i = 0; i < n; ++i) { + auto &fromVertex = vertices[i]; + for (std::size_t j = 0; j < n; ++j) { + auto &toVertex = vertices[j]; + ILineString line{fromVertex, toVertex}; + if (bg::covered_by(line, intArea)) { + connectionGraph(i, j) = bg::length(line); + } else { + connectionGraph(i, j) = std::numeric_limits::infinity(); + } + } + } +#ifdef SNAKE_DEBUG + std::cout << "connection grah:" << std::endl; + std::cout << connectionGraph << std::endl; +#endif + + // Create distance matrix. + auto distLambda = [&connectionGraph](std::size_t i, std::size_t j) -> double { + return connectionGraph(i, j); + }; + auto nNodes = nodeList.size(); + Matrix distanceMatrix(nNodes, nNodes); + for (std::size_t i = 0; i < nNodes; ++i) { + distanceMatrix(i, i) = 0; + for (std::size_t j = i + 1; j < nNodes; ++j) { + auto dist = connectionGraph(i, j); + if (std::isinf(dist)) { + std::vector route; + if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) { + std::stringstream ss; + ss << "Distance matrix calculation failed. connection graph: " + << connectionGraph << std::endl; + ss << "area: " << bg::wkt(area) << std::endl; + ss << "transects:" << std::endl; + for (const auto &t : transects) { + + ss << bg::wkt(t) << std::endl; + } + + par.errorString = ss.str(); + return false; + } + (void)route; + } + distanceMatrix(i, j) = dist; + distanceMatrix(j, i) = dist; + } + } +#ifdef SNAKE_DEBUG + std::cout << "distance matrix:" << std::endl; + std::cout << distanceMatrix << std::endl; +#endif + + // Create (asymmetric) routing matrix. + Matrix routingMatrix(nNodes, nNodes); + for (std::size_t i = 0; i < nNodes; ++i) { + auto fromNode = nodeList[i]; + for (std::size_t j = 0; j < nNodes; ++j) { + auto toNode = nodeList[j]; + routingMatrix(i, j) = distanceMatrix(fromNode.fromIndex, toNode.toIndex); + } + } + // Insert max for disjoint nodes. + for (const auto &d : disjointNodes) { + auto i = d.first; + auto j = d.second; + routingMatrix(i, j) = std::numeric_limits::max(); + routingMatrix(j, i) = std::numeric_limits::max(); + } +#ifdef SNAKE_DEBUG + std::cout << "routing matrix:" << std::endl; + std::cout << routingMatrix << std::endl; +#endif + + // Create Routing Index Manager. + auto minNumTransectsPerRun = + std::max(1, par.minNumTransectsPerRun); + auto maxRuns = std::max( + 1, std::floor(double(transects.size()) / minNumTransectsPerRun)); + auto numRuns = std::max(1, par.numRuns); + numRuns = std::min(numRuns, maxRuns); + + RoutingIndexManager::NodeIndex depot(0); + // std::vector depots(numRuns, depot); + // RoutingIndexManager manager(nNodes, numRuns, depots, depots); + RoutingIndexManager manager(nNodes, numRuns, depot); + + // Create Routing Model. + RoutingModel routing(manager); + // Create and register a transit callback. + const int transitCallbackIndex = routing.RegisterTransitCallback( + [&routingMatrix, &manager](int64 from_index, int64 to_index) -> int64 { + // Convert from routing variable Index to distance matrix NodeIndex. + auto from_node = manager.IndexToNode(from_index).value(); + auto to_node = manager.IndexToNode(to_index).value(); + return routingMatrix(from_node, to_node); + }); + // Define cost of each arc. + routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex); + // Add distance dimension. + if (numRuns > 1) { + routing.AddDimension(transitCallbackIndex, 0, 300000000, + true, // start cumul to zero + "Distance"); + routing.GetMutableDimension("Distance") + ->SetGlobalSpanCostCoefficient(100000000); + } + + // Define disjunctions. +#ifdef SNAKE_DEBUG + std::cout << "disjunctions:" << std::endl; +#endif + for (const auto &d : disjointNodes) { + auto i = d.first; + auto j = d.second; +#ifdef SNAKE_DEBUG + std::cout << i << "," << j << std::endl; +#endif + auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i)); + auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(j)); + std::vector disj{idx0, idx1}; + routing.AddDisjunction(disj, -1 /*force cardinality*/, 1 /*cardinality*/); + } + + // Set first solution heuristic. + auto searchParameters = DefaultRoutingSearchParameters(); + searchParameters.set_first_solution_strategy( + FirstSolutionStrategy::PATH_CHEAPEST_ARC); + // Number of solutions. + auto numSolutionsPerRun = std::max(1, par.numSolutionsPerRun); + searchParameters.set_number_of_solutions_to_collect(numSolutionsPerRun); + // Set costume limit. + auto *solver = routing.solver(); + auto *limit = solver->MakeCustomLimit(par.stop); + routing.AddSearchMonitor(limit); +#ifdef SNAKE_SHOW_TIME + auto delta = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start); + cout << "create routing model: " << delta.count() << " ms" << endl; +#endif + + //================================================================ + // Solve model. + //================================================================ +#ifdef SNAKE_SHOW_TIME + start = std::chrono::high_resolution_clock::now(); +#endif + auto pSolutions = std::make_unique>(); + (void)routing.SolveWithParameters(searchParameters, pSolutions.get()); +#ifdef SNAKE_SHOW_TIME + delta = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start); + cout << "solve routing model: " << delta.count() << " ms" << endl; +#endif + if (par.stop()) { + par.errorString = "User terminated."; + return false; + } + if (pSolutions->size() == 0) { + std::stringstream ss; + ss << "No solution found." << std::endl; + par.errorString = ss.str(); + return false; + } + + //================================================================ + // Construc route. + //================================================================ +#ifdef SNAKE_SHOW_TIME + start = std::chrono::high_resolution_clock::now(); +#endif + long long counter = -1; + // Note: route number 0 corresponds to the best route which is the last entry + // of *pSolutions. + for (auto solution = pSolutions->end() - 1; solution >= pSolutions->begin(); + --solution) { + ++counter; + if (!*solution || (*solution)->Size() <= 1) { + std::stringstream ss; + ss << par.errorString << "Solution " << counter << "invalid." + << std::endl; + par.errorString = ss.str(); + continue; + } + // Iterate over all routes. + Solution routeVector; + for (std::size_t vehicle = 0; vehicle < numRuns; ++vehicle) { + if (!routing.IsVehicleUsed(**solution, vehicle)) + continue; + + // Create index list. + auto index = routing.Start(vehicle); + std::vector route_idx; + route_idx.push_back(manager.IndexToNode(index).value()); + while (!routing.IsEnd(index)) { + index = (*solution)->Value(routing.NextVar(index)); + route_idx.push_back(manager.IndexToNode(index).value()); + } + +#ifdef SNAKE_DEBUG + // Print route. + std::cout << "route " << counter + << " route_idx.size() = " << route_idx.size() << std::endl; + std::cout << "route: "; + for (const auto &idx : route_idx) { + std::cout << idx << ", "; + } + std::cout << std::endl; +#endif + if (route_idx.size() < 2) { + std::stringstream ss; + ss << par.errorString + << "Error while assembling route (solution = " << counter + << ", run = " << vehicle << ")." << std::endl; + par.errorString = ss.str(); + continue; + } + + // Assemble route. + Route r; + auto &path = r.path; + auto &info = r.info; + for (size_t i = 0; i < route_idx.size() - 1; ++i) { + size_t nodeIndex0 = route_idx[i]; + size_t nodeIndex1 = route_idx[i + 1]; + const auto &n2t0 = nodeToTransectList[nodeIndex0]; + info.emplace_back(n2t0.transectsIndex, n2t0.reversed); + // Copy transect to route. + const auto &t = transects[n2t0.transectsIndex]; + if (n2t0.reversed) { // transect reversal needed? + for (auto it = t.end() - 1; it > t.begin(); --it) { + path.push_back(*it); + } + } else { + for (auto it = t.begin(); it < t.end() - 1; ++it) { + path.push_back(*it); + } + } + // Connect transects. + std::vector idxList; + if (!shortestPathFromGraph(connectionGraph, + nodeList[nodeIndex0].fromIndex, + nodeList[nodeIndex1].toIndex, idxList)) { + std::stringstream ss; + ss << par.errorString + << "Error while assembling route (solution = " << counter + << ", run = " << vehicle << ")." << std::endl; + par.errorString = ss.str(); + continue; + } + if (i != route_idx.size() - 2) { + idxList.pop_back(); + } + for (auto idx : idxList) { + auto p = int2Float(vertices[idx]); + path.push_back(p); + } + } + // Append last transect info. + const auto &n2t0 = nodeToTransectList.back(); + info.emplace_back(n2t0.transectsIndex, n2t0.reversed); + + if (path.size() < 2 || info.size() < 2) { + std::stringstream ss; + ss << par.errorString << "Route empty (solution = " << counter + << ", run = " << vehicle << ")." << std::endl; + par.errorString = ss.str(); + continue; + } + + routeVector.push_back(std::move(r)); + } + if (routeVector.size() > 0) { + solutionVector.push_back(std::move(routeVector)); + } else { + std::stringstream ss; + ss << par.errorString << "Solution " << counter << " empty." << std::endl; + par.errorString = ss.str(); + } + } +#ifdef SNAKE_SHOW_TIME + delta = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start); + cout << "reconstruct route: " << delta.count() << " ms" << endl; +#endif + + if (solutionVector.size() > 0) { + return true; + } else { + return false; + } +} diff --git a/src/MeasurementComplexItem/RoutingThread.h b/src/MeasurementComplexItem/RoutingThread.h index 79b97cab862ef424506ee52b6bfe2130991fb597..65a64cf2b0651abae67a883e68cb620a5232a708 100644 --- a/src/MeasurementComplexItem/RoutingThread.h +++ b/src/MeasurementComplexItem/RoutingThread.h @@ -4,24 +4,40 @@ #include #include -#include "geometry/snake.h" +#include "geometry/geometry.h" #include #include #include #include -struct RoutingData { - snake::Transects transects; - std::vector solutionVector; +// Aux structs +struct TransectInfo { + TransectInfo(size_t n, bool r) : index(n), reversed(r) {} + size_t index; + bool reversed; +}; + +struct Route { + geometry::FLineString path; + std::vector info; +}; + +typedef std::vector + Solution; // Every route corresponds to one run/vehicle + +struct RoutingResult { + geometry::LineStringArray transects; + std::vector solutionVector; std::string errorString; }; struct RoutingParameter { RoutingParameter() : numSolutions(1), numRuns(1) {} - snake::FPolygon safeArea; + geometry::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 @@ -31,9 +47,8 @@ class RoutingThread : public QThread { using Lock = std::unique_lock; public: - using PtrRoutingData = shared_ptr; - using Generator = std::function; - using Consumer = std::function; + using PtrRoutingData = std::shared_ptr; + using Work = std::function; RoutingThread(QObject *parent = nullptr); ~RoutingThread() override; @@ -41,7 +56,7 @@ public: bool calculating() const; public slots: - void route(const RoutingParameter &par, const Generator &generator); + void route(const RoutingParameter &par, const Work &work); signals: void result(PtrRoutingData pTransects); @@ -55,7 +70,7 @@ private: mutable std::condition_variable _cv; // Internal data RoutingParameter _par; - Generator _generator; // transect generator + Work _work; // transect worker // State std::atomic_bool _calculating; std::atomic_bool _stop; diff --git a/src/MeasurementComplexItem/geometry/GeoArea.cc b/src/MeasurementComplexItem/geometry/GeoArea.cc index aebdeefe9c83c69a7da67eed54c262e13eb1be24..a385b7444130dd82f02a5552079c62ab01b60901 100644 --- a/src/MeasurementComplexItem/geometry/GeoArea.cc +++ b/src/MeasurementComplexItem/geometry/GeoArea.cc @@ -1,6 +1,6 @@ #include "GeoArea.h" -#include "snake.h" +#include "geometry.h" #include "QGCLoggingCategory.h" #include "QGCQGeoCoordinate.h" @@ -48,17 +48,19 @@ bool GeoArea::loadFromJson(const QJsonObject &json, QString &errorString) { bool GeoArea::isCorrect() { if (this->pathModel().count() >= 3) { auto origin = this->pathModel().value(0)->coordinate(); - snake::FPolygon polygonENU; - snake::areaToEnu(origin, this->pathModel(), polygonENU); + geometry::FPolygon polygonENU; + geometry::areaToEnu(origin, this->pathModel(), polygonENU); std::string msg; if (bg::is_valid(polygonENU, msg)) { return true; } else { - qCWarning(GeoAreaLog) << msg.c_str(); - qCWarning(GeoAreaLog) << "origin: " << origin; + qCWarning(GeoAreaLog) << "isCorrect(): " << msg.c_str(); + qCWarning(GeoAreaLog) << "isCorrect(): " + << "origin: " << origin; std::stringstream ss; ss << bg::wkt(polygonENU); - qCWarning(GeoAreaLog) << "polygonENU: " << ss.str().c_str(); + qCWarning(GeoAreaLog) << "isCorrect(): " + << "polygonENU: " << ss.str().c_str(); setErrorString(this->objectName() + tr(" must be a simple polygon.")); } } @@ -70,10 +72,10 @@ QString GeoArea::errorString() const { return this->_errorString; } bool GeoArea::covers(const QGeoCoordinate &c) { if (GeoArea::isCorrect()) { auto origin = this->pathModel().value(0)->coordinate(); - snake::FPolygon polygonENU; - snake::areaToEnu(origin, this->pathModel(), polygonENU); - snake::FPoint cENU; - snake::toENU(origin, c, cENU); + geometry::FPolygon polygonENU; + geometry::areaToEnu(origin, this->pathModel(), polygonENU); + geometry::FPoint cENU; + geometry::toENU(origin, c, cENU); return bg::covered_by(cENU, polygonENU); } else { return false; @@ -100,7 +102,7 @@ void GeoArea::setErrorString(const QString &str) { emit errorStringChanged(); } -void GeoArea::setErrorString(const string &str) { +void GeoArea::setErrorString(const std::string &str) { this->_errorString = str.c_str(); emit errorStringChanged(); } diff --git a/src/MeasurementComplexItem/geometry/MeasurementArea.cc b/src/MeasurementComplexItem/geometry/MeasurementArea.cc index 80f53ff251d0b91b124bc30a39703c875ef153bb..7d4169475c7ed324fc3e5638a6506571287d3584 100644 --- a/src/MeasurementComplexItem/geometry/MeasurementArea.cc +++ b/src/MeasurementComplexItem/geometry/MeasurementArea.cc @@ -1,7 +1,7 @@ #include "MeasurementArea.h" #include "QtConcurrentRun" +#include "geometry.h" #include "nemo_interface/SnakeTile.h" -#include "snake.h" #include #include @@ -15,6 +15,14 @@ #define SNAKE_MAX_TILES 1000 #endif +using namespace geometry; +namespace trans = bg::strategy::transform; + +// Aux function +bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth, + Area minTileArea, std::vector &tiles, + BoundingBox &bbox); + QGC_LOGGING_CATEGORY(MeasurementAreaLog, "MeasurementAreaLog") namespace { @@ -460,7 +468,7 @@ void MeasurementArea::resetProgress() { //! \pre MeasurementArea::deferUpdate must be called first, don't call //! this function directly! void MeasurementArea::doUpdate() { - using namespace snake; + using namespace geometry; using namespace boost::units; auto start = std::chrono::high_resolution_clock::now(); @@ -493,10 +501,8 @@ void MeasurementArea::doUpdate() { areaToEnu(origin, polygon, polygonENU); std::vector tilesENU; BoundingBox bbox; - std::string errorString; // Generate tiles. - if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox, - errorString)) { + if (getTiles(polygonENU, height, width, minArea, tilesENU, bbox)) { // Convert to geo system. for (const auto &t : tilesENU) { auto geoTile = new SnakeTile(pData.get()); @@ -507,8 +513,8 @@ void MeasurementArea::doUpdate() { } pData->tiles.append(geoTile); // Calculate center. - snake::FPoint center; - snake::polygonCenter(t, center); + geometry::FPoint center; + geometry::polygonCenter(t, center); QGeoCoordinate geoCenter; fromENU(origin, center, geoCenter); pData->tileCenterPoints.append(QVariant::fromValue(geoCenter)); @@ -625,3 +631,106 @@ void MeasurementArea::setHoldProgress(bool holdProgress) { emit holdProgressChanged(); } } + +bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth, + Area minTileArea, std::vector &tiles, + BoundingBox &bbox) { + if (area.outer().empty() || area.outer().size() < 4) { + qCDebug(MeasurementAreaLog) << "Area has to few vertices."; + return false; + } + + if (tileWidth <= 0 * bu::si::meter || tileHeight <= 0 * bu::si::meter || + minTileArea < 0 * bu::si::meter * bu::si::meter) { + std::stringstream ss; + ss << "Parameters tileWidth (" << tileWidth << "), tileHeight (" + << tileHeight << "), minTileArea (" << minTileArea + << ") must be positive."; + qCDebug(MeasurementAreaLog) << ss.str().c_str(); + return false; + } + + if (bbox.corners.outer().size() != 5) { + bbox.corners.clear(); + minimalBoundingBox(area, bbox); + } + + if (bbox.corners.outer().size() < 5) + return false; + double bboxWidth = bbox.width; + double bboxHeight = bbox.height; + FPoint origin = bbox.corners.outer()[0]; + + // cout << "Origin: " << origin[0] << " " << origin[1] << endl; + // Transform _mArea polygon to bounding box coordinate system. + trans::rotate_transformer rotate( + bbox.angle * 180 / M_PI); + trans::translate_transformer translate(-origin.get<0>(), + -origin.get<1>()); + FPolygon translated_polygon; + FPolygon rotated_polygon; + boost::geometry::transform(area, translated_polygon, translate); + boost::geometry::transform(translated_polygon, rotated_polygon, rotate); + bg::correct(rotated_polygon); + // cout << bg::wkt(rotated_polygon) << endl; + + size_t iMax = ceil(bboxWidth / tileWidth.value()); + size_t jMax = ceil(bboxHeight / tileHeight.value()); + + if (iMax < 1 || jMax < 1) { + std::stringstream ss; + ss << "Tile width (" << tileWidth << ") or tile height (" << tileHeight + << ") to large for measurement area."; + qCDebug(MeasurementAreaLog) << ss.str().c_str(); + return false; + } + + trans::rotate_transformer rotate_back( + -bbox.angle * 180 / M_PI); + trans::translate_transformer translate_back(origin.get<0>(), + origin.get<1>()); + for (size_t i = 0; i < iMax; ++i) { + double x_min = tileWidth.value() * i; + double x_max = x_min + tileWidth.value(); + for (size_t j = 0; j < jMax; ++j) { + double y_min = tileHeight.value() * j; + double y_max = y_min + tileHeight.value(); + + FPolygon tile_unclipped; + tile_unclipped.outer().push_back(FPoint{x_min, y_min}); + tile_unclipped.outer().push_back(FPoint{x_min, y_max}); + tile_unclipped.outer().push_back(FPoint{x_max, y_max}); + tile_unclipped.outer().push_back(FPoint{x_max, y_min}); + tile_unclipped.outer().push_back(FPoint{x_min, y_min}); + + std::deque boost_tiles; + if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, + boost_tiles)) + continue; + + for (FPolygon t : boost_tiles) { + if (bg::area(t) > minTileArea.value()) { + // Transform boost_tile to world coordinate system. + FPolygon rotated_tile; + FPolygon translated_tile; + boost::geometry::transform(t, rotated_tile, rotate_back); + boost::geometry::transform(rotated_tile, translated_tile, + translate_back); + + // Store tile and calculate center point. + tiles.push_back(translated_tile); + } + } + } + } + + if (tiles.size() < 1) { + std::stringstream ss; + ss << "No tiles calculated. Is the minTileArea (" << minTileArea + << ") parameter large enough?"; + qCDebug(MeasurementAreaLog) << ss.str().c_str(); + return false; + } + + return true; +} diff --git a/src/MeasurementComplexItem/geometry/geometry.cpp b/src/MeasurementComplexItem/geometry/geometry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ecb8ab0af0672aa6e8ecd102c62fe5fcb22549d1 --- /dev/null +++ b/src/MeasurementComplexItem/geometry/geometry.cpp @@ -0,0 +1,528 @@ +#include +#include + +#include "geometry.h" + +#include +#include + +#include +#include +#include +#include + +#include "clipper/clipper.hpp" +#define CLIPPER_SCALE 1000000 + +#ifndef NDEBUG +//#define SNAKE_SHOW_TIME +#endif + +namespace bg = boost::geometry; +namespace trans = bg::strategy::transform; + +BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(bg::cs::cartesian) + +namespace geometry { +static const IntType stdScale = 1000000; +//========================================================================= +// Geometry stuff. +//========================================================================= + +void polygonCenter(const FPolygon &polygon, FPoint ¢er) { + using namespace mapbox; + if (polygon.outer().empty()) + return; + mapbox::geometry::polygon p; + mapbox::geometry::linear_ring lr1; + for (size_t i = 0; i < polygon.outer().size(); ++i) { + mapbox::geometry::point vertex(polygon.outer()[i].get<0>(), + polygon.outer()[i].get<1>()); + lr1.push_back(vertex); + } + p.push_back(lr1); + mapbox::geometry::point c = polylabel(p); + + center.set<0>(c.x); + center.set<1>(c.y); +} + +bool minimalBoundingBox(const FPolygon &polygon, BoundingBox &minBBox) { + /* + Find the minimum-area bounding box of a set of 2D points + + The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates. + The first and last points points must be the same, making a closed polygon. + This program finds the rotation angles of each edge of the convex polygon, + then tests the area of a bounding box aligned with the unique angles in + 90 degrees of the 1st Quadrant. + Returns the + + Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version) + Results verified using Matlab + + Copyright (c) 2013, David Butterworth, University of Queensland + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Willow Garage, Inc. nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + */ + + if (polygon.outer().empty() || polygon.outer().size() < 3) + return false; + FPolygon convex_hull; + bg::convex_hull(polygon, convex_hull); + + // cout << "Convex hull: " << bg::wkt(convex_hull) << endl; + + //# Compute edges (x2-x1,y2-y1) + std::vector edges; + const auto &convex_hull_outer = convex_hull.outer(); + for (long i = 0; i < long(convex_hull_outer.size()) - 1; ++i) { + FPoint p1 = convex_hull_outer.at(i); + FPoint p2 = convex_hull_outer.at(i + 1); + double edge_x = p2.get<0>() - p1.get<0>(); + double edge_y = p2.get<1>() - p1.get<1>(); + edges.push_back(FPoint{edge_x, edge_y}); + } + + // cout << "Edges: "; + // for (auto e : edges) + // cout << e.get<0>() << " " << e.get<1>() << ","; + // cout << endl; + + // Calculate unique edge angles atan2(y/x) + double angle_scale = 1e3; + std::set angles_long; + for (auto vertex : edges) { + double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2); + angle = + angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers + angles_long.insert(long(round(angle * angle_scale))); + } + std::vector edge_angles; + for (auto a : angles_long) + edge_angles.push_back(double(a) / angle_scale); + + // cout << "Unique angles: "; + // for (auto e : edge_angles) + // cout << e*180/M_PI << ","; + // cout << endl; + + double min_area = std::numeric_limits::infinity(); + // Test each angle to find bounding box with smallest area + // print "Testing", len(edge_angles), "possible rotations for bounding box... + // \n" + for (double angle : edge_angles) { + + trans::rotate_transformer rotate(angle * 180 / + M_PI); + FPolygon hull_rotated; + bg::transform(convex_hull, hull_rotated, rotate); + // cout << "Convex hull rotated: " << bg::wkt(hull_rotated) + // << endl; + + bg::model::box box; + bg::envelope(hull_rotated, box); + // cout << "Bounding box: " << + // bg::wkt>(box) << endl; + + //# print "Rotated hull points are \n", rot_points + FPoint min_corner = box.min_corner(); + FPoint max_corner = box.max_corner(); + double min_x = min_corner.get<0>(); + double max_x = max_corner.get<0>(); + double min_y = min_corner.get<1>(); + double max_y = max_corner.get<1>(); + // cout << "min_x: " << min_x << endl; + // cout << "max_x: " << max_x << endl; + // cout << "min_y: " << min_y << endl; + // cout << "max_y: " << max_y << endl; + + // Calculate height/width/area of this bounding rectangle + double width = max_x - min_x; + double height = max_y - min_y; + double area = width * height; + // cout << "Width: " << width << endl; + // cout << "Height: " << height << endl; + // cout << "area: " << area << endl; + // cout << "angle: " << angle*180/M_PI << endl; + + // Store the smallest rect found first (a simple convex hull might have 2 + // answers with same area) + if (area < min_area) { + min_area = area; + minBBox.angle = angle; + minBBox.width = width; + minBBox.height = height; + + minBBox.corners.clear(); + minBBox.corners.outer().push_back(FPoint{min_x, min_y}); + minBBox.corners.outer().push_back(FPoint{min_x, max_y}); + minBBox.corners.outer().push_back(FPoint{max_x, max_y}); + minBBox.corners.outer().push_back(FPoint{max_x, min_y}); + minBBox.corners.outer().push_back(FPoint{min_x, min_y}); + } + // cout << endl << endl; + } + + // Transform corners of minimal bounding box. + trans::rotate_transformer rotate(-minBBox.angle * + 180 / M_PI); + FPolygon rotated_polygon; + bg::transform(minBBox.corners, rotated_polygon, rotate); + minBBox.corners = rotated_polygon; + + return true; +} + +void offsetPolygon(const FPolygon &polygon, FPolygon &polygonOffset, + double offset) { + bg::strategy::buffer::distance_symmetric distance_strategy(offset); + bg::strategy::buffer::join_miter join_strategy(3); + bg::strategy::buffer::end_flat end_strategy; + bg::strategy::buffer::point_square point_strategy; + bg::strategy::buffer::side_straight side_strategy; + + bg::model::multi_polygon result; + + bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, + end_strategy, point_strategy); + + if (result.size() > 0) + polygonOffset = result[0]; +} + +void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices, + Matrix &graph) { + size_t n = graph.n(); + + for (size_t i = 0; i < n; ++i) { + FPoint v1 = vertices[i]; + for (size_t j = i + 1; j < n; ++j) { + FPoint v2 = vertices[j]; + FLineString path{v1, v2}; + + double distance = 0; + if (!bg::within(path, polygon)) + distance = std::numeric_limits::infinity(); + else + distance = bg::length(path); + graph(i, j) = distance; + graph(j, i) = distance; + } + } +} + +bool toDistanceMatrix(Matrix &graph) { + size_t n = graph.n(); + + auto distance = [&graph](size_t i, size_t j) -> double { + return graph(i, j); + }; + + for (size_t i = 0; i < n; ++i) { + for (size_t j = i + 1; j < n; ++j) { + double d = graph(i, j); + if (!std::isinf(d)) + continue; + std::vector path; + if (!dijkstraAlgorithm(n, i, j, path, d, distance)) { + return false; + } + // cout << "(" << i << "," << j << ") d: " << d << endl; + // cout << "Path size: " << path.size() << endl; + // for (auto idx : path) + // cout << idx << " "; + // cout << endl; + graph(i, j) = d; + graph(j, i) = d; + } + } + return true; +} + +bool joinedArea(const FPolygon &mArea, const FPolygon &sArea, + const FPolygon &corridor, FPolygon &jArea, + std::string &errorString) { + // Measurement area and service area overlapping? + bool overlapingSerMeas = bg::intersects(mArea, sArea) ? true : false; + bool corridorValid = corridor.outer().size() > 0 ? true : false; + + // Check if corridor is connecting measurement area and service area. + bool corridor_is_connection = false; + if (corridorValid) { + // Corridor overlaping with measurement area? + if (bg::intersects(corridor, mArea)) { + // Corridor overlaping with service area? + if (bg::intersects(corridor, sArea)) { + corridor_is_connection = true; + } + } + } + + // Are areas joinable? + std::deque sol; + FPolygon partialArea = mArea; + if (overlapingSerMeas) { + if (corridor_is_connection) { + bg::union_(partialArea, corridor, sol); + } + } else if (corridor_is_connection) { + bg::union_(partialArea, corridor, sol); + } else { + std::stringstream ss; + auto printPoint = [&ss](const FPoint &p) { + ss << " (" << p.get<0>() << ", " << p.get<1>() << ")"; + }; + ss << "Areas are not overlapping." << std::endl; + ss << "Measurement area:"; + bg::for_each_point(mArea, printPoint); + ss << std::endl; + ss << "Service area:"; + bg::for_each_point(sArea, printPoint); + ss << std::endl; + ss << "Corridor:"; + bg::for_each_point(corridor, printPoint); + ss << std::endl; + errorString = ss.str(); + return false; + } + + if (sol.size() > 0) { + partialArea = sol[0]; + sol.clear(); + } + + // Join areas. + bg::union_(partialArea, sArea, sol); + + if (sol.size() > 0) { + jArea = sol[0]; + } else { + std::stringstream ss; + auto printPoint = [&ss](const FPoint &p) { + ss << " (" << p.get<0>() << ", " << p.get<1>() << ")"; + }; + ss << "Areas not joinable." << std::endl; + ss << "Measurement area:"; + bg::for_each_point(mArea, printPoint); + ss << std::endl; + ss << "Service area:"; + bg::for_each_point(sArea, printPoint); + ss << std::endl; + ss << "Corridor:"; + bg::for_each_point(corridor, printPoint); + ss << std::endl; + errorString = ss.str(); + return false; + } + + return true; +} + +bool joinedArea(const std::vector &areas, FPolygon &joinedArea) { + if (areas.size() < 1) + return false; + joinedArea = *areas[0]; + std::deque idxList; + for (size_t i = 1; i < areas.size(); ++i) + idxList.push_back(i); + + std::deque sol; + while (idxList.size() > 0) { + bool success = false; + for (auto it = idxList.begin(); it != idxList.end(); ++it) { + bg::union_(joinedArea, *areas[*it], sol); + if (sol.size() > 0) { + joinedArea = sol[0]; + sol.clear(); + idxList.erase(it); + success = true; + break; + } + } + if (!success) + return false; + } + + return true; +} + +BoundingBox::BoundingBox() : width(0), height(0), angle(0) {} + +void BoundingBox::clear() { + width = 0; + height = 0; + angle = 0; + corners.clear(); +} + +FPoint int2Float(const IPoint &ip) { return int2Float(ip, stdScale); } + +FPoint int2Float(const IPoint &ip, IntType scale) { + return FPoint{FloatType(ip.get<0>()) / scale, FloatType(ip.get<1>()) / scale}; +} + +IPoint float2Int(const FPoint &ip) { return float2Int(ip, stdScale); } + +IPoint float2Int(const FPoint &ip, IntType scale) { + return IPoint{IntType(std::llround(ip.get<0>() * scale)), + IntType(std::llround(ip.get<1>() * scale))}; +} + +bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex, + std::vector &elementPath, double &length, + std::function distanceDij) { + if (startIndex >= numElements || endIndex >= numElements) { + length = std::numeric_limits::infinity(); + return false; + } else if (endIndex == startIndex) { + length = 0; + elementPath.push_back(startIndex); + return true; + } + + // Node struct + // predecessorIndex is the index of the predecessor node + // (nodeList[predecessorIndex]) distance is the distance between the node and + // the start node node number is stored by the position in nodeList + struct Node { + std::size_t predecessorIndex = std::numeric_limits::max(); + double distance = std::numeric_limits::infinity(); + }; + + // The list with all Nodes (elements) + std::vector nodeList(numElements); + // This list will be initalized with indices referring to the elements of + // nodeList. Elements will be successively remove during the execution of the + // Dijkstra Algorithm. + std::vector workingSet(numElements); + + // append elements to node list + for (size_t i = 0; i < numElements; ++i) + workingSet[i] = i; + + nodeList[startIndex].distance = 0; + + // Dijkstra Algorithm + // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus + while (workingSet.size() > 0) { + // serach Node with minimal distance + auto minDist = std::numeric_limits::infinity(); + std::size_t minDistIndex_WS = + std::numeric_limits::max(); // WS = workinSet + for (size_t i = 0; i < workingSet.size(); ++i) { + const auto nodeIndex = workingSet.at(i); + const auto dist = nodeList.at(nodeIndex).distance; + if (dist < minDist) { + minDist = dist; + minDistIndex_WS = i; + } + } + if (minDistIndex_WS == std::numeric_limits::max()) + return false; + + size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList + workingSet.erase(workingSet.begin() + minDistIndex_WS); + if (indexU_NL == endIndex) // shortest path found + break; + + const auto distanceU = nodeList.at(indexU_NL).distance; + // update distance + for (size_t i = 0; i < workingSet.size(); ++i) { + auto indexV_NL = workingSet[i]; // NL = nodeList + Node *v = &nodeList[indexV_NL]; + auto dist = distanceDij(indexU_NL, indexV_NL); + // is ther an alternative path which is shorter? + auto alternative = distanceU + dist; + if (alternative < v->distance) { + v->distance = alternative; + v->predecessorIndex = indexU_NL; + } + } + } + // end Djikstra Algorithm + + // reverse assemble path + auto e = endIndex; + length = nodeList[e].distance; + while (true) { + if (e == std::numeric_limits::max()) { + if (elementPath.size() > 0 && + elementPath[0] == startIndex) { // check if starting point was reached + break; + } else { // some error + length = std::numeric_limits::infinity(); + elementPath.clear(); + return false; + } + } else { + elementPath.insert(elementPath.begin(), e); + // Update Node + e = nodeList[e].predecessorIndex; + } + } + return true; +} + +bool shortestPathFromGraph(const Matrix &graph, const size_t startIndex, + const size_t endIndex, + std::vector &pathIdx) { + if (!std::isinf(graph(startIndex, endIndex))) { + pathIdx.push_back(startIndex); + pathIdx.push_back(endIndex); + } else { + auto distance = [&graph](size_t i, size_t j) -> double { + return graph(i, j); + }; + double d = 0; + if (!dijkstraAlgorithm(graph.n(), startIndex, endIndex, pathIdx, d, + distance)) { + return false; + } + } + return true; +} + +} // namespace geometry + +bool boost::geometry::model::operator==(::geometry::FPoint &p1, + ::geometry::FPoint &p2) { + return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>()); +} + +bool boost::geometry::model::operator!=(::geometry::FPoint &p1, + ::geometry::FPoint &p2) { + return !(p1 == p2); +} + +bool boost::geometry::model::operator==(::geometry::IPoint &p1, + ::geometry::IPoint &p2) { + return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>()); +} +bool boost::geometry::model::operator!=(::geometry::IPoint &p1, + ::geometry::IPoint &p2) { + return !(p1 == p2); +} diff --git a/src/MeasurementComplexItem/geometry/snake.h b/src/MeasurementComplexItem/geometry/geometry.h similarity index 62% rename from src/MeasurementComplexItem/geometry/snake.h rename to src/MeasurementComplexItem/geometry/geometry.h index 0450ea12f955d8bd4ab88cbd40598cc74384ef69..a1e59182e0e49cd3adfefc5b890deae14ea10f12 100644 --- a/src/MeasurementComplexItem/geometry/snake.h +++ b/src/MeasurementComplexItem/geometry/geometry.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -23,26 +24,25 @@ namespace bu = boost::units; #include "QGCQGeoCoordinate.h" #include "QmlObjectListModel.h" -using namespace std; - -namespace snake { +namespace geometry { //========================================================================= // Geometry stuff. //========================================================================= // Double geometry. -using FloatType = double; +typedef double FloatType; typedef bg::model::point FPoint; typedef bg::model::polygon FPolygon; typedef bg::model::linestring FLineString; typedef bg::model::box FBox; +typedef std::vector LineStringArray; // Integer geometry. -using IntType = long long; -using IPoint = bg::model::point; -using IPolygon = bg::model::polygon; -using IRing = bg::model::ring; -using ILineString = bg::model::linestring; +typedef long long IntType; +typedef bg::model::point IPoint; +typedef bg::model::polygon IPolygon; +typedef bg::model::ring IRing; +typedef bg::model::linestring ILineString; FPoint int2Float(const IPoint &ip); FPoint int2Float(const IPoint &ip, IntType scale); @@ -52,7 +52,7 @@ IPoint float2Int(const FPoint &ip, IntType scale); template class Matrix; template -ostream &operator<<(ostream &os, const Matrix &matrix) { +std::ostream &operator<<(std::ostream &os, const Matrix &matrix) { for (std::size_t i = 0; i < matrix.m(); ++i) { for (std::size_t j = 0; j < matrix.n(); ++j) { os << "(" << i << "," << j << "):" << matrix(i, j) << std::endl; @@ -85,7 +85,8 @@ public: std::size_t m() const { return _n; } std::size_t n() const { return _n; } - friend ostream &operator<<<>(ostream &os, const Matrix &dt); + friend std::ostream &operator<<<>(std::ostream &os, + const Matrix &dt); private: std::vector _matrix; @@ -104,46 +105,100 @@ struct BoundingBox { FPolygon corners; }; +constexpr int earth_radius = 6371000; // meters (m) +constexpr double epsilon = std::numeric_limits::epsilon(); // meters (m) + template void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) { - static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); - GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0, - earth); - double x = 0, y = 0, z = 0; - proj.Forward(in.latitude(), in.longitude(), 0, x, y, z); + double lat_rad = in.latitude() * M_PI / 180; + double lon_rad = in.longitude() * M_PI / 180; + + double ref_lon_rad = origin.longitude() * M_PI / 180; + double ref_lat_rad = origin.latitude() * M_PI / 180; + + double sin_lat = std::sin(lat_rad); + double cos_lat = std::cos(lat_rad); + double cos_d_lon = std::cos(lon_rad - ref_lon_rad); + + double ref_sin_lat = std::sin(ref_lat_rad); + double ref_cos_lat = std::cos(ref_lat_rad); + + double c = + std::acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon); + double k = (std::fabs(c) < epsilon) ? 1.0 : (c / std::sin(c)); + + double x = k * cos_lat * std::sin(lon_rad - ref_lon_rad) * earth_radius; + double y = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * + earth_radius; + out.set<0>(x); out.set<1>(y); - (void)z; } template void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) { - static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); - GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0, - earth); - double x = 0, y = 0, z = 0; - proj.Forward(in.latitude(), in.longitude(), 0, x, y, z); + using namespace std; + + double lat_rad = in.latitude() * M_PI / 180; + double lon_rad = in.longitude() * M_PI / 180; + + double ref_lon_rad = origin.longitude() * M_PI / 180; + double ref_lat_rad = origin.latitude() * M_PI / 180; + + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref_lon_rad); + + double ref_sin_lat = sin(ref_lat_rad); + double ref_cos_lat = cos(ref_lat_rad); + + double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon); + double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c)); + + double x = k * cos_lat * sin(lon_rad - ref_lon_rad) * earth_radius; + double y = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * + earth_radius; + out.setX(x); out.setY(y); - (void)z; } template void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) { - static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); - GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0, - earth); - - double lat = 0, lon = 0, alt = 0; - proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt); - out.setLatitude(lat); - out.setLongitude(lon); - out.setAltitude(alt); + + using namespace std; + + double x_rad = in.get<0>() / earth_radius; + double y_rad = in.get<1>() / earth_radius; + double c = sqrt(y_rad * y_rad + x_rad * x_rad); + double sin_c = sin(c); + double cos_c = cos(c); + + double ref_lon_rad = origin.longitude() * M_PI / 180; + double ref_lat_rad = origin.latitude() * M_PI / 180; + + double ref_sin_lat = sin(ref_lat_rad); + double ref_cos_lat = cos(ref_lat_rad); + + double lat_rad; + double lon_rad; + + if (fabs(c) > epsilon) { + lat_rad = asin(cos_c * ref_sin_lat + (y_rad * sin_c * ref_cos_lat) / c); + lon_rad = + (ref_lon_rad + atan2(x_rad * sin_c, c * ref_cos_lat * cos_c - + y_rad * ref_sin_lat * sin_c)); + + } else { + lat_rad = ref_lat_rad; + lon_rad = ref_lon_rad; + } + + out.setLatitude(lat_rad * 180 / M_PI); + out.setLongitude(lon_rad * 180 / M_PI); + out.setAltitude(origin.altitude()); } template @@ -225,59 +280,17 @@ bool joinedArea(const std::vector &areas, FPolygon &jArea); bool joinedArea(const FPolygon &mArea, const FPolygon &sArea, const FPolygon &corridor, FPolygon &jArea, std::string &errorString); -bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth, - Area minTileArea, std::vector &tiles, BoundingBox &bbox, - std::string &errorString); - -using Transects = vector; -using Progress = vector; - -bool transectsFromScenario(Length distance, Length minLength, Angle angle, - const FPolygon &mArea, - const std::vector &tiles, - const Progress &p, Transects &t, - string &errorString); - -struct TransectInfo { - TransectInfo(size_t n, bool r) : index(n), reversed(r) {} - size_t index; - bool reversed; -}; -struct Route { - FLineString path; - std::vector info; -}; -using Solution = - std::vector; // Every route corresponds to one run/vehicle -struct RouteParameter { - RouteParameter() - : numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5), - stop([] { return false; }) {} - std::size_t numSolutionsPerRun; - std::size_t numRuns; - std::size_t minNumTransectsPerRun; - std::function stop; - mutable std::string errorString; -}; -bool route(const FPolygon &area, const Transects &transects, - std::vector &solutionVector, - const RouteParameter &par = RouteParameter()); - -namespace detail { -const double offsetConstant = - 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. -} // namespace detail -} // namespace snake +} // namespace geometry // operator== and operator!= for boost point namespace boost { namespace geometry { namespace model { -bool operator==(snake::FPoint &p1, snake::FPoint &p2); -bool operator!=(snake::FPoint &p1, snake::FPoint &p2); -bool operator==(snake::IPoint &p1, snake::IPoint &p2); -bool operator!=(snake::IPoint &p1, snake::IPoint &p2); +bool operator==(::geometry::FPoint &p1, ::geometry::FPoint &p2); +bool operator!=(::geometry::FPoint &p1, ::geometry::FPoint &p2); +bool operator==(::geometry::IPoint &p1, ::geometry::IPoint &p2); +bool operator!=(::geometry::IPoint &p1, ::geometry::IPoint &p2); } // namespace model } // namespace geometry diff --git a/src/MeasurementComplexItem/geometry/snake.cpp b/src/MeasurementComplexItem/geometry/snake.cpp deleted file mode 100644 index 8886d35d43adcf3124f29f2d53702f9411a75820..0000000000000000000000000000000000000000 --- a/src/MeasurementComplexItem/geometry/snake.cpp +++ /dev/null @@ -1,1169 +0,0 @@ -#include -#include - -#include "snake.h" - -#include -#include - -#include -#include -#include -#include - -#include "clipper/clipper.hpp" -#define CLIPPER_SCALE 1000000 - -#include "ortools/constraint_solver/routing.h" -#include "ortools/constraint_solver/routing_enums.pb.h" -#include "ortools/constraint_solver/routing_index_manager.h" -#include "ortools/constraint_solver/routing_parameters.h" - -using namespace operations_research; - -#ifndef NDEBUG -//#define SNAKE_SHOW_TIME -#endif - -namespace bg = boost::geometry; -namespace trans = bg::strategy::transform; - -BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(bg::cs::cartesian) - -namespace snake { -static const IntType stdScale = 1000000; -//========================================================================= -// Geometry stuff. -//========================================================================= - -void polygonCenter(const FPolygon &polygon, FPoint ¢er) { - using namespace mapbox; - if (polygon.outer().empty()) - return; - geometry::polygon p; - geometry::linear_ring lr1; - for (size_t i = 0; i < polygon.outer().size(); ++i) { - geometry::point vertex(polygon.outer()[i].get<0>(), - polygon.outer()[i].get<1>()); - lr1.push_back(vertex); - } - p.push_back(lr1); - geometry::point c = polylabel(p); - - center.set<0>(c.x); - center.set<1>(c.y); -} - -bool minimalBoundingBox(const FPolygon &polygon, BoundingBox &minBBox) { - /* - Find the minimum-area bounding box of a set of 2D points - - The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates. - The first and last points points must be the same, making a closed polygon. - This program finds the rotation angles of each edge of the convex polygon, - then tests the area of a bounding box aligned with the unique angles in - 90 degrees of the 1st Quadrant. - Returns the - - Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version) - Results verified using Matlab - - Copyright (c) 2013, David Butterworth, University of Queensland - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the Willow Garage, Inc. nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - POSSIBILITY OF SUCH DAMAGE. - */ - - if (polygon.outer().empty() || polygon.outer().size() < 3) - return false; - FPolygon convex_hull; - bg::convex_hull(polygon, convex_hull); - - // cout << "Convex hull: " << bg::wkt(convex_hull) << endl; - - //# Compute edges (x2-x1,y2-y1) - std::vector edges; - const auto &convex_hull_outer = convex_hull.outer(); - for (long i = 0; i < long(convex_hull_outer.size()) - 1; ++i) { - FPoint p1 = convex_hull_outer.at(i); - FPoint p2 = convex_hull_outer.at(i + 1); - double edge_x = p2.get<0>() - p1.get<0>(); - double edge_y = p2.get<1>() - p1.get<1>(); - edges.push_back(FPoint{edge_x, edge_y}); - } - - // cout << "Edges: "; - // for (auto e : edges) - // cout << e.get<0>() << " " << e.get<1>() << ","; - // cout << endl; - - // Calculate unique edge angles atan2(y/x) - double angle_scale = 1e3; - std::set angles_long; - for (auto vertex : edges) { - double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2); - angle = - angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers - angles_long.insert(long(round(angle * angle_scale))); - } - std::vector edge_angles; - for (auto a : angles_long) - edge_angles.push_back(double(a) / angle_scale); - - // cout << "Unique angles: "; - // for (auto e : edge_angles) - // cout << e*180/M_PI << ","; - // cout << endl; - - double min_area = std::numeric_limits::infinity(); - // Test each angle to find bounding box with smallest area - // print "Testing", len(edge_angles), "possible rotations for bounding box... - // \n" - for (double angle : edge_angles) { - - trans::rotate_transformer rotate(angle * 180 / - M_PI); - FPolygon hull_rotated; - bg::transform(convex_hull, hull_rotated, rotate); - // cout << "Convex hull rotated: " << bg::wkt(hull_rotated) - // << endl; - - bg::model::box box; - bg::envelope(hull_rotated, box); - // cout << "Bounding box: " << - // bg::wkt>(box) << endl; - - //# print "Rotated hull points are \n", rot_points - FPoint min_corner = box.min_corner(); - FPoint max_corner = box.max_corner(); - double min_x = min_corner.get<0>(); - double max_x = max_corner.get<0>(); - double min_y = min_corner.get<1>(); - double max_y = max_corner.get<1>(); - // cout << "min_x: " << min_x << endl; - // cout << "max_x: " << max_x << endl; - // cout << "min_y: " << min_y << endl; - // cout << "max_y: " << max_y << endl; - - // Calculate height/width/area of this bounding rectangle - double width = max_x - min_x; - double height = max_y - min_y; - double area = width * height; - // cout << "Width: " << width << endl; - // cout << "Height: " << height << endl; - // cout << "area: " << area << endl; - // cout << "angle: " << angle*180/M_PI << endl; - - // Store the smallest rect found first (a simple convex hull might have 2 - // answers with same area) - if (area < min_area) { - min_area = area; - minBBox.angle = angle; - minBBox.width = width; - minBBox.height = height; - - minBBox.corners.clear(); - minBBox.corners.outer().push_back(FPoint{min_x, min_y}); - minBBox.corners.outer().push_back(FPoint{min_x, max_y}); - minBBox.corners.outer().push_back(FPoint{max_x, max_y}); - minBBox.corners.outer().push_back(FPoint{max_x, min_y}); - minBBox.corners.outer().push_back(FPoint{min_x, min_y}); - } - // cout << endl << endl; - } - - // Transform corners of minimal bounding box. - trans::rotate_transformer rotate(-minBBox.angle * - 180 / M_PI); - FPolygon rotated_polygon; - bg::transform(minBBox.corners, rotated_polygon, rotate); - minBBox.corners = rotated_polygon; - - return true; -} - -void offsetPolygon(const FPolygon &polygon, FPolygon &polygonOffset, - double offset) { - bg::strategy::buffer::distance_symmetric distance_strategy(offset); - bg::strategy::buffer::join_miter join_strategy(3); - bg::strategy::buffer::end_flat end_strategy; - bg::strategy::buffer::point_square point_strategy; - bg::strategy::buffer::side_straight side_strategy; - - bg::model::multi_polygon result; - - bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, - end_strategy, point_strategy); - - if (result.size() > 0) - polygonOffset = result[0]; -} - -void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices, - Matrix &graph) { - size_t n = graph.n(); - - for (size_t i = 0; i < n; ++i) { - FPoint v1 = vertices[i]; - for (size_t j = i + 1; j < n; ++j) { - FPoint v2 = vertices[j]; - FLineString path{v1, v2}; - - double distance = 0; - if (!bg::within(path, polygon)) - distance = std::numeric_limits::infinity(); - else - distance = bg::length(path); - graph(i, j) = distance; - graph(j, i) = distance; - } - } -} - -bool toDistanceMatrix(Matrix &graph) { - size_t n = graph.n(); - - auto distance = [&graph](size_t i, size_t j) -> double { - return graph(i, j); - }; - - for (size_t i = 0; i < n; ++i) { - for (size_t j = i + 1; j < n; ++j) { - double d = graph(i, j); - if (!std::isinf(d)) - continue; - std::vector path; - if (!dijkstraAlgorithm(n, i, j, path, d, distance)) { - return false; - } - // cout << "(" << i << "," << j << ") d: " << d << endl; - // cout << "Path size: " << path.size() << endl; - // for (auto idx : path) - // cout << idx << " "; - // cout << endl; - graph(i, j) = d; - graph(j, i) = d; - } - } - return true; -} - -bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth, - Area minTileArea, std::vector &tiles, BoundingBox &bbox, - string &errorString) { - if (area.outer().empty() || area.outer().size() < 4) { - errorString = "Area has to few vertices."; - return false; - } - - if (tileWidth <= 0 * bu::si::meter || tileHeight <= 0 * bu::si::meter || - minTileArea < 0 * bu::si::meter * bu::si::meter) { - std::stringstream ss; - ss << "Parameters tileWidth (" << tileWidth << "), tileHeight (" - << tileHeight << "), minTileArea (" << minTileArea - << ") must be positive."; - errorString = ss.str(); - return false; - } - - if (bbox.corners.outer().size() != 5) { - bbox.corners.clear(); - minimalBoundingBox(area, bbox); - } - - if (bbox.corners.outer().size() < 5) - return false; - double bboxWidth = bbox.width; - double bboxHeight = bbox.height; - FPoint origin = bbox.corners.outer()[0]; - - // cout << "Origin: " << origin[0] << " " << origin[1] << endl; - // Transform _mArea polygon to bounding box coordinate system. - trans::rotate_transformer rotate( - bbox.angle * 180 / M_PI); - trans::translate_transformer translate(-origin.get<0>(), - -origin.get<1>()); - FPolygon translated_polygon; - FPolygon rotated_polygon; - boost::geometry::transform(area, translated_polygon, translate); - boost::geometry::transform(translated_polygon, rotated_polygon, rotate); - bg::correct(rotated_polygon); - // cout << bg::wkt(rotated_polygon) << endl; - - size_t iMax = ceil(bboxWidth / tileWidth.value()); - size_t jMax = ceil(bboxHeight / tileHeight.value()); - - if (iMax < 1 || jMax < 1) { - std::stringstream ss; - ss << "Tile width (" << tileWidth << ") or tile height (" << tileHeight - << ") to large for measurement area."; - errorString = ss.str(); - return false; - } - - trans::rotate_transformer rotate_back( - -bbox.angle * 180 / M_PI); - trans::translate_transformer translate_back(origin.get<0>(), - origin.get<1>()); - for (size_t i = 0; i < iMax; ++i) { - double x_min = tileWidth.value() * i; - double x_max = x_min + tileWidth.value(); - for (size_t j = 0; j < jMax; ++j) { - double y_min = tileHeight.value() * j; - double y_max = y_min + tileHeight.value(); - - FPolygon tile_unclipped; - tile_unclipped.outer().push_back(FPoint{x_min, y_min}); - tile_unclipped.outer().push_back(FPoint{x_min, y_max}); - tile_unclipped.outer().push_back(FPoint{x_max, y_max}); - tile_unclipped.outer().push_back(FPoint{x_max, y_min}); - tile_unclipped.outer().push_back(FPoint{x_min, y_min}); - - std::deque boost_tiles; - if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, - boost_tiles)) - continue; - - for (FPolygon t : boost_tiles) { - if (bg::area(t) > minTileArea.value()) { - // Transform boost_tile to world coordinate system. - FPolygon rotated_tile; - FPolygon translated_tile; - boost::geometry::transform(t, rotated_tile, rotate_back); - boost::geometry::transform(rotated_tile, translated_tile, - translate_back); - - // Store tile and calculate center point. - tiles.push_back(translated_tile); - } - } - } - } - - if (tiles.size() < 1) { - std::stringstream ss; - ss << "No tiles calculated. Is the minTileArea (" << minTileArea - << ") parameter large enough?"; - errorString = ss.str(); - return false; - } - - return true; -} - -bool joinedArea(const FPolygon &mArea, const FPolygon &sArea, - const FPolygon &corridor, FPolygon &jArea, - std::string &errorString) { - // Measurement area and service area overlapping? - bool overlapingSerMeas = bg::intersects(mArea, sArea) ? true : false; - bool corridorValid = corridor.outer().size() > 0 ? true : false; - - // Check if corridor is connecting measurement area and service area. - bool corridor_is_connection = false; - if (corridorValid) { - // Corridor overlaping with measurement area? - if (bg::intersects(corridor, mArea)) { - // Corridor overlaping with service area? - if (bg::intersects(corridor, sArea)) { - corridor_is_connection = true; - } - } - } - - // Are areas joinable? - std::deque sol; - FPolygon partialArea = mArea; - if (overlapingSerMeas) { - if (corridor_is_connection) { - bg::union_(partialArea, corridor, sol); - } - } else if (corridor_is_connection) { - bg::union_(partialArea, corridor, sol); - } else { - std::stringstream ss; - auto printPoint = [&ss](const FPoint &p) { - ss << " (" << p.get<0>() << ", " << p.get<1>() << ")"; - }; - ss << "Areas are not overlapping." << std::endl; - ss << "Measurement area:"; - bg::for_each_point(mArea, printPoint); - ss << std::endl; - ss << "Service area:"; - bg::for_each_point(sArea, printPoint); - ss << std::endl; - ss << "Corridor:"; - bg::for_each_point(corridor, printPoint); - ss << std::endl; - errorString = ss.str(); - return false; - } - - if (sol.size() > 0) { - partialArea = sol[0]; - sol.clear(); - } - - // Join areas. - bg::union_(partialArea, sArea, sol); - - if (sol.size() > 0) { - jArea = sol[0]; - } else { - std::stringstream ss; - auto printPoint = [&ss](const FPoint &p) { - ss << " (" << p.get<0>() << ", " << p.get<1>() << ")"; - }; - ss << "Areas not joinable." << std::endl; - ss << "Measurement area:"; - bg::for_each_point(mArea, printPoint); - ss << std::endl; - ss << "Service area:"; - bg::for_each_point(sArea, printPoint); - ss << std::endl; - ss << "Corridor:"; - bg::for_each_point(corridor, printPoint); - ss << std::endl; - errorString = ss.str(); - return false; - } - - return true; -} - -bool joinedArea(const std::vector &areas, FPolygon &joinedArea) { - if (areas.size() < 1) - return false; - joinedArea = *areas[0]; - std::deque idxList; - for (size_t i = 1; i < areas.size(); ++i) - idxList.push_back(i); - - std::deque sol; - while (idxList.size() > 0) { - bool success = false; - for (auto it = idxList.begin(); it != idxList.end(); ++it) { - bg::union_(joinedArea, *areas[*it], sol); - if (sol.size() > 0) { - joinedArea = sol[0]; - sol.clear(); - idxList.erase(it); - success = true; - break; - } - } - if (!success) - return false; - } - - return true; -} - -BoundingBox::BoundingBox() : width(0), height(0), angle(0) {} - -void BoundingBox::clear() { - width = 0; - height = 0; - angle = 0; - corners.clear(); -} - -bool transectsFromScenario(Length distance, Length minLength, Angle angle, - const FPolygon &mArea, - const std::vector &tiles, - const Progress &p, Transects &t, - string &errorString) { - // Rotate measurement area by angle and calculate bounding box. - FPolygon mAreaRotated; - trans::rotate_transformer rotate(angle.value() * - 180 / M_PI); - bg::transform(mArea, mAreaRotated, rotate); - - FBox box; - boost::geometry::envelope(mAreaRotated, box); - double x0 = box.min_corner().get<0>(); - double y0 = box.min_corner().get<1>(); - double x1 = box.max_corner().get<0>(); - double y1 = box.max_corner().get<1>(); - - // Generate transects and convert them to clipper path. - size_t num_t = int(ceil((y1 - y0) / distance.value())); // number of transects - vector transectsClipper; - transectsClipper.reserve(num_t); - for (size_t i = 0; i < num_t; ++i) { - // calculate transect - FPoint v1{x0, y0 + i * distance.value()}; - FPoint v2{x1, y0 + i * distance.value()}; - FLineString transect; - transect.push_back(v1); - transect.push_back(v2); - // transform back - FLineString temp_transect; - trans::rotate_transformer 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; - errorString = ss.str(); - return false; - } - - // Convert measurement area to clipper path. - ClipperLib::Path mAreaClipper; - for (auto vertex : mArea.outer()) { - mAreaClipper.push_back(ClipperLib::IntPoint{ - static_cast(vertex.get<0>() * CLIPPER_SCALE), - static_cast(vertex.get<1>() * CLIPPER_SCALE)}); - } - - // Perform clipping. - // Clip transects to measurement area. - ClipperLib::Clipper clipper; - clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true); - clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false); - ClipperLib::PolyTree clippedTransecs; - clipper.Execute(ClipperLib::ctIntersection, clippedTransecs, - ClipperLib::pftNonZero, ClipperLib::pftNonZero); - const auto *transects = &clippedTransecs; - - bool ignoreProgress = p.size() != tiles.size(); - ClipperLib::PolyTree clippedTransecs2; - if (!ignoreProgress) { - // Calculate processed tiles (_progress[i] == 100) and subtract them from - // measurement area. - size_t numTiles = p.size(); - vector processedTiles; - for (size_t i = 0; i < numTiles; ++i) { - if (p[i] == 100) { - processedTiles.push_back(tiles[i]); - } - } - - if (processedTiles.size() != numTiles) { - vector processedTilesClipper; - for (const auto &t : processedTiles) { - ClipperLib::Path path; - for (const auto &vertex : t.outer()) { - path.push_back(ClipperLib::IntPoint{ - static_cast(vertex.get<0>() * CLIPPER_SCALE), - static_cast(vertex.get<1>() * CLIPPER_SCALE)}); - } - processedTilesClipper.push_back(path); - } - - // Subtract holes (tiles with measurement_progress == 100) from transects. - clipper.Clear(); - for (const auto &child : clippedTransecs.Childs) { - clipper.AddPath(child->Contour, ClipperLib::ptSubject, false); - } - clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true); - clipper.Execute(ClipperLib::ctDifference, clippedTransecs2, - ClipperLib::pftNonZero, ClipperLib::pftNonZero); - transects = &clippedTransecs2; - } else { - // All tiles processed (t.size() not changed). - return true; - } - } - - // Extract transects from PolyTree and convert them to BoostLineString - for (const auto &child : transects->Childs) { - const auto &clipperTransect = child->Contour; - FPoint v1{static_cast(clipperTransect[0].X) / CLIPPER_SCALE, - static_cast(clipperTransect[0].Y) / CLIPPER_SCALE}; - FPoint v2{static_cast(clipperTransect[1].X) / CLIPPER_SCALE, - static_cast(clipperTransect[1].Y) / CLIPPER_SCALE}; - - FLineString transect{v1, v2}; - if (bg::length(transect) >= minLength.value()) { - t.push_back(transect); - } - } - - if (t.size() == 0) { - std::stringstream ss; - ss << "Not able to generate transects. Parameter: minLength = " << minLength - << std::endl; - errorString = ss.str(); - return false; - } - return true; -} - -bool route(const FPolygon &area, const Transects &transects, - std::vector &solutionVector, const RouteParameter &par) { - -#ifdef SNAKE_SHOW_TIME - auto start = std::chrono::high_resolution_clock::now(); -#endif - //================================================================ - // Create routing model. - //================================================================ - // Use integer polygons to increase numerical robustness. - // Convert area; - IPolygon intArea; - for (const auto &v : area.outer()) { - auto p = float2Int(v); - intArea.outer().push_back(p); - } - for (const auto &ring : area.inners()) { - IRing intRing; - for (const auto &v : ring) { - auto p = float2Int(v); - intRing.push_back(p); - } - intArea.inners().push_back(std::move(intRing)); - } - - // Helper classes. - struct VirtualNode { - VirtualNode(std::size_t f, std::size_t t) : fromIndex(f), toIndex(t) {} - std::size_t fromIndex; // index for leaving node - std::size_t toIndex; // index for entering node - }; - struct NodeToTransect { - NodeToTransect(std::size_t i, bool r) : transectsIndex(i), reversed(r) {} - std::size_t transectsIndex; // transects index - bool reversed; // transect reversed? - }; - // Create vertex and node list - std::vector vertices; - std::vector> disjointNodes; - std::vector nodeList; - std::vector nodeToTransectList; - for (std::size_t i = 0; i < transects.size(); ++i) { - const auto &t = transects[i]; - // Copy line edges only. - if (t.size() == 1 || i == 0) { - auto p = float2Int(t.back()); - vertices.push_back(p); - nodeToTransectList.emplace_back(i, false); - auto idx = vertices.size() - 1; - nodeList.emplace_back(idx, idx); - } else if (t.size() > 1) { - auto p1 = float2Int(t.front()); - auto p2 = float2Int(t.back()); - vertices.push_back(p1); - vertices.push_back(p2); - nodeToTransectList.emplace_back(i, false); - nodeToTransectList.emplace_back(i, true); - auto fromIdx = vertices.size() - 1; - auto toIdx = fromIdx - 1; - nodeList.emplace_back(fromIdx, toIdx); - nodeList.emplace_back(toIdx, fromIdx); - disjointNodes.emplace_back(toIdx, fromIdx); - } else { // transect empty - std::cout << "ignoring empty transect with index " << i << std::endl; - } - } -#ifdef SNAKE_DEBUG - // Print. - std::cout << "nodeToTransectList:" << std::endl; - std::cout << "node:transectIndex:reversed" << std::endl; - std::size_t c = 0; - for (const auto &n2t : nodeToTransectList) { - std::cout << c++ << ":" << n2t.transectsIndex << ":" << n2t.reversed - << std::endl; - } - std::cout << "nodeList:" << std::endl; - std::cout << "node:fromIndex:toIndex" << std::endl; - c = 0; - for (const auto &n : nodeList) { - std::cout << c++ << ":" << n.fromIndex << ":" << n.toIndex << std::endl; - } - std::cout << "disjoint nodes:" << std::endl; - std::cout << "number:nodes" << std::endl; - c = 0; - for (const auto &d : disjointNodes) { - std::cout << c++ << ":" << d.first << "," << d.second << std::endl; - } -#endif - - // Add polygon vertices. - for (auto &v : intArea.outer()) { - vertices.push_back(v); - } - for (auto &ring : intArea.inners()) { - for (auto &v : ring) { - vertices.push_back(v); - } - } - - // Create connection graph (inf == no connection between vertices). - // Note: graph is not symmetric. - auto n = vertices.size(); - // Matrix must be double since integers don't have infinity and nan - Matrix connectionGraph(n, n); - for (std::size_t i = 0; i < n; ++i) { - auto &fromVertex = vertices[i]; - for (std::size_t j = 0; j < n; ++j) { - auto &toVertex = vertices[j]; - ILineString line{fromVertex, toVertex}; - if (bg::covered_by(line, intArea)) { - connectionGraph(i, j) = bg::length(line); - } else { - connectionGraph(i, j) = std::numeric_limits::infinity(); - } - } - } -#ifdef SNAKE_DEBUG - std::cout << "connection grah:" << std::endl; - std::cout << connectionGraph << std::endl; -#endif - - // Create distance matrix. - auto distLambda = [&connectionGraph](std::size_t i, std::size_t j) -> double { - return connectionGraph(i, j); - }; - auto nNodes = nodeList.size(); - Matrix distanceMatrix(nNodes, nNodes); - for (std::size_t i = 0; i < nNodes; ++i) { - distanceMatrix(i, i) = 0; - for (std::size_t j = i + 1; j < nNodes; ++j) { - auto dist = connectionGraph(i, j); - if (std::isinf(dist)) { - std::vector route; - if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) { - std::stringstream ss; - ss << "Distance matrix calculation failed. connection graph: " - << connectionGraph << std::endl; - ss << "area: " << bg::wkt(area) << std::endl; - ss << "transects:" << std::endl; - for (const auto &t : transects) { - - ss << bg::wkt(t) << std::endl; - } - - par.errorString = ss.str(); - return false; - } - (void)route; - } - distanceMatrix(i, j) = dist; - distanceMatrix(j, i) = dist; - } - } -#ifdef SNAKE_DEBUG - std::cout << "distance matrix:" << std::endl; - std::cout << distanceMatrix << std::endl; -#endif - - // Create (asymmetric) routing matrix. - Matrix routingMatrix(nNodes, nNodes); - for (std::size_t i = 0; i < nNodes; ++i) { - auto fromNode = nodeList[i]; - for (std::size_t j = 0; j < nNodes; ++j) { - auto toNode = nodeList[j]; - routingMatrix(i, j) = distanceMatrix(fromNode.fromIndex, toNode.toIndex); - } - } - // Insert max for disjoint nodes. - for (const auto &d : disjointNodes) { - auto i = d.first; - auto j = d.second; - routingMatrix(i, j) = std::numeric_limits::max(); - routingMatrix(j, i) = std::numeric_limits::max(); - } -#ifdef SNAKE_DEBUG - std::cout << "routing matrix:" << std::endl; - std::cout << routingMatrix << std::endl; -#endif - - // Create Routing Index Manager. - auto minNumTransectsPerRun = - std::max(1, par.minNumTransectsPerRun); - auto maxRuns = std::max( - 1, std::floor(double(transects.size()) / minNumTransectsPerRun)); - auto numRuns = std::max(1, par.numRuns); - numRuns = std::min(numRuns, maxRuns); - - RoutingIndexManager::NodeIndex depot(0); - // std::vector depots(numRuns, depot); - // RoutingIndexManager manager(nNodes, numRuns, depots, depots); - RoutingIndexManager manager(nNodes, numRuns, depot); - - // Create Routing Model. - RoutingModel routing(manager); - // Create and register a transit callback. - const int transitCallbackIndex = routing.RegisterTransitCallback( - [&routingMatrix, &manager](int64 from_index, int64 to_index) -> int64 { - // Convert from routing variable Index to distance matrix NodeIndex. - auto from_node = manager.IndexToNode(from_index).value(); - auto to_node = manager.IndexToNode(to_index).value(); - return routingMatrix(from_node, to_node); - }); - // Define cost of each arc. - routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex); - // Add distance dimension. - if (numRuns > 1) { - routing.AddDimension(transitCallbackIndex, 0, 300000000, - true, // start cumul to zero - "Distance"); - routing.GetMutableDimension("Distance") - ->SetGlobalSpanCostCoefficient(100000000); - } - - // Define disjunctions. -#ifdef SNAKE_DEBUG - std::cout << "disjunctions:" << std::endl; -#endif - for (const auto &d : disjointNodes) { - auto i = d.first; - auto j = d.second; -#ifdef SNAKE_DEBUG - std::cout << i << "," << j << std::endl; -#endif - auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i)); - auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(j)); - std::vector disj{idx0, idx1}; - routing.AddDisjunction(disj, -1 /*force cardinality*/, 1 /*cardinality*/); - } - - // Set first solution heuristic. - auto searchParameters = DefaultRoutingSearchParameters(); - searchParameters.set_first_solution_strategy( - FirstSolutionStrategy::PATH_CHEAPEST_ARC); - // Number of solutions. - auto numSolutionsPerRun = std::max(1, par.numSolutionsPerRun); - searchParameters.set_number_of_solutions_to_collect(numSolutionsPerRun); - // Set costume limit. - auto *solver = routing.solver(); - auto *limit = solver->MakeCustomLimit(par.stop); - routing.AddSearchMonitor(limit); -#ifdef SNAKE_SHOW_TIME - auto delta = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start); - cout << "create routing model: " << delta.count() << " ms" << endl; -#endif - - //================================================================ - // Solve model. - //================================================================ -#ifdef SNAKE_SHOW_TIME - start = std::chrono::high_resolution_clock::now(); -#endif - auto pSolutions = std::make_unique>(); - (void)routing.SolveWithParameters(searchParameters, pSolutions.get()); -#ifdef SNAKE_SHOW_TIME - delta = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start); - cout << "solve routing model: " << delta.count() << " ms" << endl; -#endif - if (par.stop()) { - par.errorString = "User terminated."; - return false; - } - if (pSolutions->size() == 0) { - std::stringstream ss; - ss << "No solution found." << std::endl; - par.errorString = ss.str(); - return false; - } - - //================================================================ - // Construc route. - //================================================================ -#ifdef SNAKE_SHOW_TIME - start = std::chrono::high_resolution_clock::now(); -#endif - long long counter = -1; - // Note: route number 0 corresponds to the best route which is the last entry - // of *pSolutions. - for (auto solution = pSolutions->end() - 1; solution >= pSolutions->begin(); - --solution) { - ++counter; - if (!*solution || (*solution)->Size() <= 1) { - std::stringstream ss; - ss << par.errorString << "Solution " << counter << "invalid." - << std::endl; - par.errorString = ss.str(); - continue; - } - // Iterate over all routes. - Solution routeVector; - for (std::size_t vehicle = 0; vehicle < numRuns; ++vehicle) { - if (!routing.IsVehicleUsed(**solution, vehicle)) - continue; - - // Create index list. - auto index = routing.Start(vehicle); - std::vector route_idx; - route_idx.push_back(manager.IndexToNode(index).value()); - while (!routing.IsEnd(index)) { - index = (*solution)->Value(routing.NextVar(index)); - route_idx.push_back(manager.IndexToNode(index).value()); - } - -#ifdef SNAKE_DEBUG - // Print route. - std::cout << "route " << counter - << " route_idx.size() = " << route_idx.size() << std::endl; - std::cout << "route: "; - for (const auto &idx : route_idx) { - std::cout << idx << ", "; - } - std::cout << std::endl; -#endif - if (route_idx.size() < 2) { - std::stringstream ss; - ss << par.errorString - << "Error while assembling route (solution = " << counter - << ", run = " << vehicle << ")." << std::endl; - par.errorString = ss.str(); - continue; - } - - // Assemble route. - Route r; - auto &path = r.path; - auto &info = r.info; - for (size_t i = 0; i < route_idx.size() - 1; ++i) { - size_t nodeIndex0 = route_idx[i]; - size_t nodeIndex1 = route_idx[i + 1]; - const auto &n2t0 = nodeToTransectList[nodeIndex0]; - info.emplace_back(n2t0.transectsIndex, n2t0.reversed); - // Copy transect to route. - const auto &t = transects[n2t0.transectsIndex]; - if (n2t0.reversed) { // transect reversal needed? - for (auto it = t.end() - 1; it > t.begin(); --it) { - path.push_back(*it); - } - } else { - for (auto it = t.begin(); it < t.end() - 1; ++it) { - path.push_back(*it); - } - } - // Connect transects. - std::vector idxList; - if (!shortestPathFromGraph(connectionGraph, - nodeList[nodeIndex0].fromIndex, - nodeList[nodeIndex1].toIndex, idxList)) { - std::stringstream ss; - ss << par.errorString - << "Error while assembling route (solution = " << counter - << ", run = " << vehicle << ")." << std::endl; - par.errorString = ss.str(); - continue; - } - if (i != route_idx.size() - 2) { - idxList.pop_back(); - } - for (auto idx : idxList) { - auto p = int2Float(vertices[idx]); - path.push_back(p); - } - } - // Append last transect info. - const auto &n2t0 = nodeToTransectList.back(); - info.emplace_back(n2t0.transectsIndex, n2t0.reversed); - - if (path.size() < 2 || info.size() < 2) { - std::stringstream ss; - ss << par.errorString << "Route empty (solution = " << counter - << ", run = " << vehicle << ")." << std::endl; - par.errorString = ss.str(); - continue; - } - - routeVector.push_back(std::move(r)); - } - if (routeVector.size() > 0) { - solutionVector.push_back(std::move(routeVector)); - } else { - std::stringstream ss; - ss << par.errorString << "Solution " << counter << " empty." << std::endl; - par.errorString = ss.str(); - } - } -#ifdef SNAKE_SHOW_TIME - delta = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start); - cout << "reconstruct route: " << delta.count() << " ms" << endl; -#endif - - if (solutionVector.size() > 0) { - return true; - } else { - return false; - } -} - -FPoint int2Float(const IPoint &ip) { return int2Float(ip, stdScale); } - -FPoint int2Float(const IPoint &ip, IntType scale) { - return FPoint{FloatType(ip.get<0>()) / scale, FloatType(ip.get<1>()) / scale}; -} - -IPoint float2Int(const FPoint &ip) { return float2Int(ip, stdScale); } - -IPoint float2Int(const FPoint &ip, IntType scale) { - return IPoint{IntType(std::llround(ip.get<0>() * scale)), - IntType(std::llround(ip.get<1>() * scale))}; -} - -bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex, - std::vector &elementPath, double &length, - std::function distanceDij) { - if (startIndex >= numElements || endIndex >= numElements) { - length = std::numeric_limits::infinity(); - return false; - } else if (endIndex == startIndex) { - length = 0; - elementPath.push_back(startIndex); - return true; - } - - // Node struct - // predecessorIndex is the index of the predecessor node - // (nodeList[predecessorIndex]) distance is the distance between the node and - // the start node node number is stored by the position in nodeList - struct Node { - std::size_t predecessorIndex = std::numeric_limits::max(); - double distance = std::numeric_limits::infinity(); - }; - - // The list with all Nodes (elements) - std::vector nodeList(numElements); - // This list will be initalized with indices referring to the elements of - // nodeList. Elements will be successively remove during the execution of the - // Dijkstra Algorithm. - std::vector workingSet(numElements); - - // append elements to node list - for (size_t i = 0; i < numElements; ++i) - workingSet[i] = i; - - nodeList[startIndex].distance = 0; - - // Dijkstra Algorithm - // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus - while (workingSet.size() > 0) { - // serach Node with minimal distance - auto minDist = std::numeric_limits::infinity(); - std::size_t minDistIndex_WS = - std::numeric_limits::max(); // WS = workinSet - for (size_t i = 0; i < workingSet.size(); ++i) { - const auto nodeIndex = workingSet.at(i); - const auto dist = nodeList.at(nodeIndex).distance; - if (dist < minDist) { - minDist = dist; - minDistIndex_WS = i; - } - } - if (minDistIndex_WS == std::numeric_limits::max()) - return false; - - size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList - workingSet.erase(workingSet.begin() + minDistIndex_WS); - if (indexU_NL == endIndex) // shortest path found - break; - - const auto distanceU = nodeList.at(indexU_NL).distance; - // update distance - for (size_t i = 0; i < workingSet.size(); ++i) { - auto indexV_NL = workingSet[i]; // NL = nodeList - Node *v = &nodeList[indexV_NL]; - auto dist = distanceDij(indexU_NL, indexV_NL); - // is ther an alternative path which is shorter? - auto alternative = distanceU + dist; - if (alternative < v->distance) { - v->distance = alternative; - v->predecessorIndex = indexU_NL; - } - } - } - // end Djikstra Algorithm - - // reverse assemble path - auto e = endIndex; - length = nodeList[e].distance; - while (true) { - if (e == std::numeric_limits::max()) { - if (elementPath.size() > 0 && - elementPath[0] == startIndex) { // check if starting point was reached - break; - } else { // some error - length = std::numeric_limits::infinity(); - elementPath.clear(); - return false; - } - } else { - elementPath.insert(elementPath.begin(), e); - // Update Node - e = nodeList[e].predecessorIndex; - } - } - return true; -} - -bool shortestPathFromGraph(const Matrix &graph, const size_t startIndex, - const size_t endIndex, - std::vector &pathIdx) { - if (!std::isinf(graph(startIndex, endIndex))) { - pathIdx.push_back(startIndex); - pathIdx.push_back(endIndex); - } else { - auto distance = [&graph](size_t i, size_t j) -> double { - return graph(i, j); - }; - double d = 0; - if (!dijkstraAlgorithm(graph.n(), startIndex, endIndex, pathIdx, d, - distance)) { - return false; - } - } - return true; -} - -} // namespace snake - -bool boost::geometry::model::operator==(snake::FPoint &p1, snake::FPoint &p2) { - return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>()); -} - -bool boost::geometry::model::operator!=(snake::FPoint &p1, snake::FPoint &p2) { - return !(p1 == p2); -} - -bool boost::geometry::model::operator==(snake::IPoint &p1, snake::IPoint &p2) { - return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>()); -} -bool boost::geometry::model::operator!=(snake::IPoint &p1, snake::IPoint &p2) { - return !(p1 == p2); -}