Commit ed47290e authored by Valentin Platzgummer's avatar Valentin Platzgummer

libGeographic dependency removed, snake namespace renamed to geometry,...

libGeographic dependency removed, snake namespace renamed to geometry, geometry.h and geometry.cpp cleared up
parent ec7b37e1
...@@ -35,6 +35,8 @@ linux { ...@@ -35,6 +35,8 @@ linux {
QMAKE_CXXFLAGS_WARN_ON += -Werror \ QMAKE_CXXFLAGS_WARN_ON += -Werror \
-Wno-deprecated-copy \ # These come from mavlink headers -Wno-deprecated-copy \ # These come from mavlink headers
-Wno-unused-parameter \ # gst_plugins-good has these errors -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 -Wno-implicit-fallthrough # gst_plugins-good has these errors
} }
} else : linux-rasp-pi2-g++ { } else : linux-rasp-pi2-g++ {
......
...@@ -445,6 +445,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) { ...@@ -445,6 +445,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) {
# #
HEADERS += \ HEADERS += \
src/MeasurementComplexItem/geometry/geometry.h \
src/QmlControls/QmlUnitsConversion.h \ src/QmlControls/QmlUnitsConversion.h \
src/MeasurementComplexItem/geometry/GeoArea.h \ src/MeasurementComplexItem/geometry/GeoArea.h \
src/MeasurementComplexItem/geometry/MeasurementArea.h \ src/MeasurementComplexItem/geometry/MeasurementArea.h \
...@@ -479,7 +480,6 @@ HEADERS += \ ...@@ -479,7 +480,6 @@ HEADERS += \
src/MeasurementComplexItem/geometry/mapbox/recursive_wrapper.hpp \ src/MeasurementComplexItem/geometry/mapbox/recursive_wrapper.hpp \
src/MeasurementComplexItem/geometry/mapbox/variant.hpp \ src/MeasurementComplexItem/geometry/mapbox/variant.hpp \
src/MeasurementComplexItem/geometry/mapbox/variant_io.hpp \ src/MeasurementComplexItem/geometry/mapbox/variant_io.hpp \
src/MeasurementComplexItem/geometry/snake.h \
src/MeasurementComplexItem/geometry/GenericPolygon.h \ src/MeasurementComplexItem/geometry/GenericPolygon.h \
src/MeasurementComplexItem/geometry/GenericPolygonArray.h \ src/MeasurementComplexItem/geometry/GenericPolygonArray.h \
src/MeasurementComplexItem/geometry/GeoPoint3D.h \ src/MeasurementComplexItem/geometry/GeoPoint3D.h \
...@@ -524,6 +524,7 @@ SOURCES += \ ...@@ -524,6 +524,7 @@ SOURCES += \
src/MeasurementComplexItem/geometry/GeoArea.cc \ src/MeasurementComplexItem/geometry/GeoArea.cc \
src/MeasurementComplexItem/geometry/MeasurementArea.cc \ src/MeasurementComplexItem/geometry/MeasurementArea.cc \
src/MeasurementComplexItem/geometry/SafeArea.cc \ src/MeasurementComplexItem/geometry/SafeArea.cc \
src/MeasurementComplexItem/geometry/geometry.cpp \
src/Vehicle/VehicleEscStatusFactGroup.cc \ src/Vehicle/VehicleEscStatusFactGroup.cc \
src/MeasurementComplexItem/AreaData.cc \ src/MeasurementComplexItem/AreaData.cc \
src/api/QGCCorePlugin.cc \ src/api/QGCCorePlugin.cc \
...@@ -537,7 +538,6 @@ SOURCES += \ ...@@ -537,7 +538,6 @@ SOURCES += \
src/MeasurementComplexItem/GeneratorBase.cc \ src/MeasurementComplexItem/GeneratorBase.cc \
src/MeasurementComplexItem/LinearGenerator.cpp \ src/MeasurementComplexItem/LinearGenerator.cpp \
src/MeasurementComplexItem/geometry/clipper/clipper.cpp \ src/MeasurementComplexItem/geometry/clipper/clipper.cpp \
src/MeasurementComplexItem/geometry/snake.cpp \
src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \ src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \
src/MeasurementComplexItem/NemoInterface.cpp \ src/MeasurementComplexItem/NemoInterface.cpp \
src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc \ src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc \
......
...@@ -2,7 +2,7 @@ ...@@ -2,7 +2,7 @@
#include "geometry/MeasurementArea.h" #include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h" #include "geometry/SafeArea.h"
#include "geometry/snake.h" #include "geometry/geometry.h"
#include "JsonHelper.h" #include "JsonHelper.h"
#include "QGCApplication.h" #include "QGCApplication.h"
...@@ -116,10 +116,10 @@ bool AreaData::isCorrect(bool showError) { ...@@ -116,10 +116,10 @@ bool AreaData::isCorrect(bool showError) {
return false; return false;
} }
const auto &origin = this->origin(); const auto &origin = this->origin();
snake::FPolygon safeAreaENU; geometry::FPolygon safeAreaENU;
snake::areaToEnu(origin, safeArea->pathModel(), safeAreaENU); geometry::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
snake::FPolygon measurementAreaENU; geometry::FPolygon measurementAreaENU;
snake::areaToEnu(origin, measurementArea->pathModel(), measurementAreaENU); geometry::areaToEnu(origin, measurementArea->pathModel(), measurementAreaENU);
// qDebug() << "origin" << origin; // qDebug() << "origin" << origin;
// std::stringstream ss; // std::stringstream ss;
// ss << "measurementAreaENU: " << bg::wkt(measurementAreaENU) << std::endl; // ss << "measurementAreaENU: " << bg::wkt(measurementAreaENU) << std::endl;
...@@ -218,14 +218,14 @@ void AreaData::intersection(bool showError) { ...@@ -218,14 +218,14 @@ void AreaData::intersection(bool showError) {
// convert to ENU // convert to ENU
const auto origin = this->origin(); const auto origin = this->origin();
snake::FPolygon safeAreaENU; geometry::FPolygon safeAreaENU;
snake::areaToEnu(origin, safeArea->pathModel(), safeAreaENU); geometry::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
snake::FPolygon measurementAreaENU; geometry::FPolygon measurementAreaENU;
snake::areaToEnu(origin, measurementArea->pathModel(), geometry::areaToEnu(origin, measurementArea->pathModel(),
measurementAreaENU); measurementAreaENU);
// do intersection // do intersection
std::deque<snake::FPolygon> outputENU; std::deque<geometry::FPolygon> outputENU;
boost::geometry::intersection(measurementAreaENU, safeAreaENU, outputENU); boost::geometry::intersection(measurementAreaENU, safeAreaENU, outputENU);
if (outputENU.size() < 1 || outputENU[0].outer().size() < 4) { if (outputENU.size() < 1 || outputENU[0].outer().size() < 4) {
...@@ -246,9 +246,9 @@ void AreaData::intersection(bool showError) { ...@@ -246,9 +246,9 @@ void AreaData::intersection(bool showError) {
// Shrink the result if safeAreaENU doesn't cover it. // Shrink the result if safeAreaENU doesn't cover it.
auto large = std::move(outputENU[0]); auto large = std::move(outputENU[0]);
snake::FPolygon small; geometry::FPolygon small;
while (!bg::covered_by(large, safeAreaENU)) { while (!bg::covered_by(large, safeAreaENU)) {
snake::offsetPolygon(large, small, -0.1); geometry::offsetPolygon(large, small, -0.1);
large = std::move(small); large = std::move(small);
} }
...@@ -259,7 +259,7 @@ void AreaData::intersection(bool showError) { ...@@ -259,7 +259,7 @@ void AreaData::intersection(bool showError) {
for (auto it = large.outer().begin(); it != large.outer().end() - 1; for (auto it = large.outer().begin(); it != large.outer().end() - 1;
++it) { ++it) {
QGeoCoordinate c; QGeoCoordinate c;
snake::fromENU(origin, *it, c); geometry::fromENU(origin, *it, c);
measurementArea->appendVertex(c); measurementArea->appendVertex(c);
} }
} }
......
...@@ -26,7 +26,7 @@ public: ...@@ -26,7 +26,7 @@ public:
virtual QString abbreviation() const override; virtual QString abbreviation() const override;
virtual QString type() const override; virtual QString type() const override;
virtual bool get(Generator &generator) override; virtual bool get(Work &work) override;
QGeoCoordinate reference() const; QGeoCoordinate reference() const;
Fact *distance(); Fact *distance();
......
...@@ -7,7 +7,7 @@ ...@@ -7,7 +7,7 @@
#include <functional> #include <functional>
#include <memory> #include <memory>
#include "geometry/snake.h" #include "geometry/geometry.h"
#include "AreaData.h" #include "AreaData.h"
...@@ -17,7 +17,7 @@ class GeneratorBase : public QObject { ...@@ -17,7 +17,7 @@ class GeneratorBase : public QObject {
Q_OBJECT Q_OBJECT
public: public:
using Data = AreaData *; using Data = AreaData *;
using Generator = std::function<bool(snake::Transects &)>; using Work = std::function<bool(geometry::LineStringArray &)>;
explicit GeneratorBase(QObject *parent = nullptr); explicit GeneratorBase(QObject *parent = nullptr);
explicit GeneratorBase(Data d, QObject *parent = nullptr); explicit GeneratorBase(Data d, QObject *parent = nullptr);
...@@ -39,7 +39,7 @@ public: ...@@ -39,7 +39,7 @@ public:
virtual QString abbreviation() const = 0; virtual QString abbreviation() const = 0;
virtual QString type() const = 0; virtual QString type() const = 0;
virtual bool get(Generator &generator) = 0; virtual bool get(Work &work) = 0;
Data data() const; Data data() const;
void setData(Data d); void setData(Data d);
......
...@@ -23,10 +23,11 @@ const char *minLengthKey = "MinLength"; ...@@ -23,10 +23,11 @@ const char *minLengthKey = "MinLength";
QGC_LOGGING_CATEGORY(LinearGeneratorLog, "LinearGeneratorLog") QGC_LOGGING_CATEGORY(LinearGeneratorLog, "LinearGeneratorLog")
bool linearTransects(const snake::FPolygon &polygon, bool linearTransects(const geometry::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles, const std::vector<geometry::FPolygon> &tiles,
snake::Length distance, snake::Angle angle, geometry::Length distance, geometry::Angle angle,
snake::Length minLength, snake::Transects &transects); geometry::Length minLength,
geometry::LineStringArray &transects);
const char *LinearGenerator::settingsGroup = "LinearGenerator"; const char *LinearGenerator::settingsGroup = "LinearGenerator";
const char *LinearGenerator::typeString = "LinearGenerator"; const char *LinearGenerator::typeString = "LinearGenerator";
...@@ -67,7 +68,7 @@ QString LinearGenerator::abbreviation() const { ...@@ -67,7 +68,7 @@ QString LinearGenerator::abbreviation() const {
QString LinearGenerator::type() const { return typeString; } QString LinearGenerator::type() const { return typeString; }
bool LinearGenerator::get(Generator &generator) { bool LinearGenerator::get(Work &generator) {
if (_d != nullptr) { if (_d != nullptr) {
if (this->_d->isCorrect()) { if (this->_d->isCorrect()) {
// Prepare data. // Prepare data.
...@@ -95,13 +96,13 @@ bool LinearGenerator::get(Generator &generator) { ...@@ -95,13 +96,13 @@ bool LinearGenerator::get(Generator &generator) {
return false; return false;
} }
} }
auto pPolygon = std::make_shared<snake::FPolygon>(); auto pPolygon = std::make_shared<geometry::FPolygon>();
snake::areaToEnu(origin, geoPolygon, *pPolygon); geometry::areaToEnu(origin, geoPolygon, *pPolygon);
// Progress and tiles. // Progress and tiles.
const auto &progress = measurementArea->progress(); const auto &progress = measurementArea->progress();
const auto *tiles = measurementArea->tiles(); const auto *tiles = measurementArea->tiles();
auto pTiles = std::make_shared<std::vector<snake::FPolygon>>(); auto pTiles = std::make_shared<std::vector<geometry::FPolygon>>();
if (progress.size() == tiles->count()) { if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) { for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) { if (progress[i] == 100) {
...@@ -109,8 +110,8 @@ bool LinearGenerator::get(Generator &generator) { ...@@ -109,8 +110,8 @@ bool LinearGenerator::get(Generator &generator) {
const auto *tile = qobject_cast<const SnakeTile *>(obj); const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) { if (tile != nullptr) {
snake::FPolygon tileENU; geometry::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU); geometry::areaToEnu(origin, tile->coordinateList(), tileENU);
pTiles->push_back(std::move(tileENU)); pTiles->push_back(std::move(tileENU));
} else { } else {
qCDebug(LinearGeneratorLog) << "get(): tile == nullptr"; qCDebug(LinearGeneratorLog) << "get(): tile == nullptr";
...@@ -134,21 +135,21 @@ bool LinearGenerator::get(Generator &generator) { ...@@ -134,21 +135,21 @@ bool LinearGenerator::get(Generator &generator) {
qCDebug(LinearGeneratorLog) << "get(): depot invalid." << geoDepot; qCDebug(LinearGeneratorLog) << "get(): depot invalid." << geoDepot;
return false; return false;
} }
snake::FPoint depot; geometry::FPoint depot;
snake::toENU(origin, geoDepot, depot); geometry::toENU(origin, geoDepot, depot);
// Fetch transect parameter. // Fetch transect parameter.
auto distance = auto distance = geometry::Length(this->_distance.rawValue().toDouble() *
snake::Length(this->_distance.rawValue().toDouble() * bu::si::meter); bu::si::meter);
auto minLength = auto minLength = geometry::Length(this->_minLength.rawValue().toDouble() *
snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter); bu::si::meter);
auto alpha = auto alpha = geometry::Angle(this->_alpha.rawValue().toDouble() *
snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree); bu::degree::degree);
generator = [depot, pPolygon, pTiles, distance, alpha, generator = [depot, pPolygon, pTiles, distance, alpha,
minLength](snake::Transects &transects) -> bool { minLength](geometry::LineStringArray &transects) -> bool {
bool value = linearTransects(*pPolygon, *pTiles, distance, alpha, bool value = linearTransects(*pPolygon, *pTiles, distance, alpha,
minLength, transects); minLength, transects);
transects.insert(transects.begin(), snake::FLineString{depot}); transects.insert(transects.begin(), geometry::FLineString{depot});
return value; return value;
}; };
return true; return true;
...@@ -326,10 +327,11 @@ void LinearGenerator::setMeasurementArea(MeasurementArea *area) { ...@@ -326,10 +327,11 @@ void LinearGenerator::setMeasurementArea(MeasurementArea *area) {
} }
} }
bool linearTransects(const snake::FPolygon &polygon, bool linearTransects(const geometry::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles, const std::vector<geometry::FPolygon> &tiles,
snake::Length distance, snake::Angle angle, geometry::Length distance, geometry::Angle angle,
snake::Length minLength, snake::Transects &transects) { geometry::Length minLength,
geometry::LineStringArray &transects) {
namespace tr = bg::strategy::transform; namespace tr = bg::strategy::transform;
auto s1 = std::chrono::high_resolution_clock::now(); auto s1 = std::chrono::high_resolution_clock::now();
...@@ -349,9 +351,9 @@ bool linearTransects(const snake::FPolygon &polygon, ...@@ -349,9 +351,9 @@ bool linearTransects(const snake::FPolygon &polygon,
tr::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() * tr::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
180 / M_PI); 180 / M_PI);
// Rotate polygon by angle and calculate bounding box. // Rotate polygon by angle and calculate bounding box.
snake::FPolygon polygonENURotated; geometry::FPolygon polygonENURotated;
bg::transform(polygon.outer(), polygonENURotated.outer(), rotate); bg::transform(polygon.outer(), polygonENURotated.outer(), rotate);
snake::FBox box; geometry::FBox box;
boost::geometry::envelope(polygonENURotated, box); boost::geometry::envelope(polygonENURotated, box);
double x0 = box.min_corner().get<0>(); double x0 = box.min_corner().get<0>();
double y0 = box.min_corner().get<1>(); double y0 = box.min_corner().get<1>();
...@@ -360,17 +362,17 @@ bool linearTransects(const snake::FPolygon &polygon, ...@@ -360,17 +362,17 @@ bool linearTransects(const snake::FPolygon &polygon,
// Generate transects and convert them to clipper path. // Generate transects and convert them to clipper path.
size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects
vector<ClipperLib::Path> transectsClipper; std::vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t); transectsClipper.reserve(num_t);
for (size_t i = 0; i < num_t; ++i) { for (size_t i = 0; i < num_t; ++i) {
// calculate transect // calculate transect
snake::FPoint v1{x0, y0 + i * distance.value()}; geometry::FPoint v1{x0, y0 + i * distance.value()};
snake::FPoint v2{x1, y0 + i * distance.value()}; geometry::FPoint v2{x1, y0 + i * distance.value()};
snake::FLineString transect; geometry::FLineString transect;
transect.push_back(v1); transect.push_back(v1);
transect.push_back(v2); transect.push_back(v2);
// transform back // transform back
snake::FLineString temp_transect; geometry::FLineString temp_transect;
tr::rotate_transformer<bg::degree, double, 2, 2> rotate_back( tr::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
-angle.value() * 180 / M_PI); -angle.value() * 180 / M_PI);
bg::transform(transect, temp_transect, rotate_back); bg::transform(transect, temp_transect, rotate_back);
...@@ -397,8 +399,8 @@ bool linearTransects(const snake::FPolygon &polygon, ...@@ -397,8 +399,8 @@ bool linearTransects(const snake::FPolygon &polygon,
} }
// Convert measurement area to clipper path. // Convert measurement area to clipper path.
snake::FPolygon shrinked; geometry::FPolygon shrinked;
snake::offsetPolygon(polygon, shrinked, -0.2); geometry::offsetPolygon(polygon, shrinked, -0.2);
auto &outer = shrinked.outer(); auto &outer = shrinked.outer();
ClipperLib::Path polygonClipper; ClipperLib::Path polygonClipper;
for (auto vertex : outer) { for (auto vertex : outer) {
...@@ -418,7 +420,7 @@ bool linearTransects(const snake::FPolygon &polygon, ...@@ -418,7 +420,7 @@ bool linearTransects(const snake::FPolygon &polygon,
// Subtract holes. // Subtract holes.
if (tiles.size() > 0) { if (tiles.size() > 0) {
vector<ClipperLib::Path> processedTiles; std::vector<ClipperLib::Path> processedTiles;
for (const auto &tile : tiles) { for (const auto &tile : tiles) {
ClipperLib::Path path; ClipperLib::Path path;
for (const auto &v : tile.outer()) { for (const auto &v : tile.outer()) {
...@@ -442,14 +444,14 @@ bool linearTransects(const snake::FPolygon &polygon, ...@@ -442,14 +444,14 @@ bool linearTransects(const snake::FPolygon &polygon,
// Extract transects from PolyTree and convert them to BoostLineString // Extract transects from PolyTree and convert them to BoostLineString
for (const auto &child : clippedTransecs.Childs) { for (const auto &child : clippedTransecs.Childs) {
const auto &clipperTransect = child->Contour; const auto &clipperTransect = child->Contour;
snake::FPoint v1{ geometry::FPoint v1{
static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE, static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE}; static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
snake::FPoint v2{ geometry::FPoint v2{
static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE, static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE}; static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};
snake::FLineString transect{v1, v2}; geometry::FLineString transect{v1, v2};
if (bg::length(transect) >= minLength.value()) { if (bg::length(transect) >= minLength.value()) {
transects.push_back(transect); transects.push_back(transect);
} }
......
...@@ -25,7 +25,7 @@ public: ...@@ -25,7 +25,7 @@ public:
virtual QString abbreviation() const override; virtual QString abbreviation() const override;
virtual QString type() const override; virtual QString type() const override;
virtual bool get(Generator &generator) override; virtual bool get(Work &generator) override;
//! //!
//! \brief save Saves the generator. //! \brief save Saves the generator.
......
...@@ -7,7 +7,7 @@ ...@@ -7,7 +7,7 @@
#include "geometry/MeasurementArea.h" #include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h" #include "geometry/SafeArea.h"
#include "geometry/clipper/clipper.hpp" #include "geometry/clipper/clipper.hpp"
#include "geometry/snake.h" #include "geometry/geometry.h"
#include "nemo_interface/SnakeTile.h" #include "nemo_interface/SnakeTile.h"
// QGC // QGC
...@@ -365,13 +365,13 @@ bool MeasurementComplexItem::load(const QJsonObject &complexObject, ...@@ -365,13 +365,13 @@ bool MeasurementComplexItem::load(const QJsonObject &complexObject,
auto safeArea = safeAreaArray[0]; auto safeArea = safeAreaArray[0];
QGeoCoordinate origin = QGeoCoordinate origin =
safeArea->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate(); safeArea->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate();
snake::FPolygon safeAreaENU; geometry::FPolygon safeAreaENU;
snake::areaToEnu(origin, safeArea->coordinateList(), safeAreaENU); geometry::areaToEnu(origin, safeArea->coordinateList(), safeAreaENU);
for (const auto &variant : variantVector) { for (const auto &variant : variantVector) {
snake::FLineString varENU; geometry::FLineString varENU;
for (const auto &vertex : variant) { for (const auto &vertex : variant) {
snake::FPoint vertexENU; geometry::FPoint vertexENU;
snake::toENU(origin, vertex.value<QGeoCoordinate>(), vertexENU); geometry::toENU(origin, vertex.value<QGeoCoordinate>(), vertexENU);
varENU.push_back(vertexENU); varENU.push_back(vertexENU);
} }
...@@ -833,11 +833,11 @@ void MeasurementComplexItem::_updateRoute() { ...@@ -833,11 +833,11 @@ void MeasurementComplexItem::_updateRoute() {
RoutingParameter par; RoutingParameter par;
par.numSolutions = 5; par.numSolutions = 5;
auto &safeAreaENU = par.safeArea; auto &safeAreaENU = par.safeArea;
snake::areaToEnu(origin, geoSafeArea, safeAreaENU); geometry::areaToEnu(origin, geoSafeArea, safeAreaENU);
// Create generator. // Create generator.
if (this->_pGenerator != nullptr) { if (this->_pGenerator != nullptr) {
routing::GeneratorBase::Generator g; // Transect generator. routing::GeneratorBase::Work g; // Transect generator.
if (this->_pGenerator->get(g)) { if (this->_pGenerator->get(g)) {
// Start/Restart routing worker. // Start/Restart routing worker.
this->_pWorker->route(par, g); this->_pWorker->route(par, g);
...@@ -1182,7 +1182,7 @@ void MeasurementComplexItem::_storeRoutingData( ...@@ -1182,7 +1182,7 @@ void MeasurementComplexItem::_storeRoutingData(
auto ori = this->_pAreaData->origin(); auto ori = this->_pAreaData->origin();
ori.setAltitude(0); ori.setAltitude(0);
QVector<Variant> variantVector; QVector<Variant> variantVector;
const auto nSolutions = pRoute->solutionVector.size(); const std::size_t nSolutions = pRoute->solutionVector.size();
for (std::size_t j = 0; j < nSolutions; ++j) { for (std::size_t j = 0; j < nSolutions; ++j) {
Variant var; Variant var;
...@@ -1195,7 +1195,7 @@ void MeasurementComplexItem::_storeRoutingData( ...@@ -1195,7 +1195,7 @@ void MeasurementComplexItem::_storeRoutingData(
for (const auto &vertex : path) { for (const auto &vertex : path) {
QGeoCoordinate c; QGeoCoordinate c;
snake::fromENU(ori, vertex, c); geometry::fromENU(ori, vertex, c);
var.append(QVariant::fromValue(c)); var.append(QVariant::fromValue(c));
} }
} else { } else {
......
...@@ -12,7 +12,7 @@ ...@@ -12,7 +12,7 @@
#include "AreaData.h" #include "AreaData.h"
class RoutingThread; class RoutingThread;
class RoutingData; class RoutingResult;
namespace routing { namespace routing {
class GeneratorBase; class GeneratorBase;
...@@ -23,7 +23,7 @@ class MeasurementComplexItem : public ComplexMissionItem { ...@@ -23,7 +23,7 @@ class MeasurementComplexItem : public ComplexMissionItem {
using PtrGenerator = routing::GeneratorBase *; using PtrGenerator = routing::GeneratorBase *;
using PtrAreaData = AreaData *; using PtrAreaData = AreaData *;
using PtrRoutingData = std::shared_ptr<RoutingData>; using PtrRoutingData = std::shared_ptr<RoutingResult>;
using PtrWorker = RoutingThread *; using PtrWorker = RoutingThread *;
using Variant = QVariantList; using Variant = QVariantList;
......
...@@ -14,7 +14,7 @@ ...@@ -14,7 +14,7 @@
#include "GenericSingelton.h" #include "GenericSingelton.h"
#include "geometry/MeasurementArea.h" #include "geometry/MeasurementArea.h"
#include "geometry/snake.h" #include "geometry/geometry.h"
#include "nemo_interface/QNemoHeartbeat.h" #include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/QNemoProgress.h" #include "nemo_interface/QNemoProgress.h"
#include "nemo_interface/SnakeTile.h" #include "nemo_interface/SnakeTile.h"
...@@ -170,7 +170,7 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) { ...@@ -170,7 +170,7 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
tile = qobject_cast<const SnakeTile *>(obj); tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) { if (tile != nullptr) {
SnakeTileLocal tileENU; SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path()); geometry::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU)); this->tilesENU.polygons().push_back(std::move(tileENU));
} else { } else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr."; qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
......
...@@ -4,24 +4,40 @@ ...@@ -4,24 +4,40 @@
#include <QSharedPointer> #include <QSharedPointer>
#include <QThread> #include <QThread>
#include "geometry/snake.h" #include "geometry/geometry.h"
#include <atomic> #include <atomic>
#include <condition_variable> #include <condition_variable>
#include <functional> #include <functional>
#include <mutex> #include <mutex>
struct RoutingData { // Aux structs
snake::Transects transects; struct TransectInfo {
std::vector<snake::Solution> solutionVector; TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
size_t index;
bool reversed;
};
struct Route {
geometry::FLineString path;
std::vector<TransectInfo> info;
};
typedef std::vector<Route>
Solution; // Every route corresponds to one run/vehicle
struct RoutingResult {
geometry::LineStringArray transects;
std::vector<Solution> solutionVector;
std::string errorString; std::string errorString;
}; };
struct RoutingParameter { struct RoutingParameter {
RoutingParameter() : numSolutions(1), numRuns(1) {} RoutingParameter() : numSolutions(1), numRuns(1) {}
snake::FPolygon safeArea; geometry::FPolygon safeArea;
std::size_t numSolutions; std::size_t numSolutions;
std::size_t numRuns; std::size_t numRuns;
}; };
//! //!
//! \brief The CSWorker class //! \brief The CSWorker class
//! \note Don't call QThread::start, QThread::quit, etc. onyl use Worker //! \note Don't call QThread::start, QThread::quit, etc. onyl use Worker
...@@ -31,9 +47,8 @@ class RoutingThread : public QThread { ...@@ -31,9 +47,8 @@ class RoutingThread : public QThread {
using Lock = std::unique_lock<std::mutex>; using Lock = std::unique_lock<std::mutex>;
public: public:
using PtrRoutingData = shared_ptr<RoutingData>; using PtrRoutingData = std::shared_ptr<RoutingResult>;
using Generator = std::function<bool(snake::Transects &)>; using Work = std::function<bool(geometry::LineStringArray &)>;
using Consumer = std::function<void(const RoutingData &)>;
RoutingThread(QObject *parent = nullptr); RoutingThread(QObject *parent = nullptr);
~RoutingThread() override; ~RoutingThread() override;
...@@ -41,7 +56,7 @@ public: ...@@ -41,7 +56,7 @@ public:
bool calculating() const; bool calculating() const;
public slots: public slots:
void route(const RoutingParameter &par, const Generator &generator); void route(const RoutingParameter &par, const Work &work);
signals: signals:
void result(PtrRoutingData pTransects); void result(PtrRoutingData pTransects);
...@@ -55,7 +70,7 @@ private: ...@@ -55,7 +70,7 @@ private:
mutable std::condition_variable _cv; mutable std::condition_variable _cv;
// Internal data // Internal data
RoutingParameter _par; RoutingParameter _par;
Generator _generator; // transect generator Work _work; // transect worker
// State // State
std::atomic_bool _calculating; std::atomic_bool _calculating;
std::atomic_bool _stop; std::atomic_bool _stop;
......
#include "GeoArea.h" #include "GeoArea.h"
#include "snake.h" #include "geometry.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
#include "QGCQGeoCoordinate.h" #include "QGCQGeoCoordinate.h"
...@@ -48,17 +48,19 @@ bool GeoArea::loadFromJson(const QJsonObject &json, QString &errorString) { ...@@ -48,17 +48,19 @@ bool GeoArea::loadFromJson(const QJsonObject &json, QString &errorString) {
bool GeoArea::isCorrect() { bool GeoArea::isCorrect() {
if (this->pathModel().count() >= 3) { if (this->pathModel().count() >= 3) {
auto origin = this->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate(); auto origin = this->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate();
snake::FPolygon polygonENU; geometry::FPolygon polygonENU;
snake::areaToEnu(origin, this->pathModel(), polygonENU); geometry::areaToEnu(origin, this->pathModel(), polygonENU);
std::string msg; std::string msg;
if (bg::is_valid(polygonENU, msg)) { if (bg::is_valid(polygonENU, msg)) {
return true; return true;
} else { } else {
qCWarning(GeoAreaLog) << msg.c_str(); qCWarning(GeoAreaLog) << "isCorrect(): " << msg.c_str();
qCWarning(GeoAreaLog) << "origin: " << origin; qCWarning(GeoAreaLog) << "isCorrect(): "
<< "origin: " << origin;
std::stringstream ss; std::stringstream ss;
ss << bg::wkt(polygonENU); 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.")); setErrorString(this->objectName() + tr(" must be a simple polygon."));
} }
} }
...@@ -70,10 +72,10 @@ QString GeoArea::errorString() const { return this->_errorString; } ...@@ -70,10 +72,10 @@ QString GeoArea::errorString() const { return this->_errorString; }
bool GeoArea::covers(const QGeoCoordinate &c) { bool GeoArea::covers(const QGeoCoordinate &c) {
if (GeoArea::isCorrect()) { if (GeoArea::isCorrect()) {
auto origin = this->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate(); auto origin = this->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate();
snake::FPolygon polygonENU; geometry::FPolygon polygonENU;
snake::areaToEnu(origin, this->pathModel(), polygonENU); geometry::areaToEnu(origin, this->pathModel(), polygonENU);
snake::FPoint cENU; geometry::FPoint cENU;
snake::toENU(origin, c, cENU); geometry::toENU(origin, c, cENU);
return bg::covered_by(cENU, polygonENU); return bg::covered_by(cENU, polygonENU);
} else { } else {
return false; return false;
...@@ -100,7 +102,7 @@ void GeoArea::setErrorString(const QString &str) { ...@@ -100,7 +102,7 @@ void GeoArea::setErrorString(const QString &str) {
emit errorStringChanged(); emit errorStringChanged();
} }
void GeoArea::setErrorString(const string &str) { void GeoArea::setErrorString(const std::string &str) {
this->_errorString = str.c_str(); this->_errorString = str.c_str();
emit errorStringChanged(); emit errorStringChanged();
} }
......
#include "MeasurementArea.h" #include "MeasurementArea.h"
#include "QtConcurrentRun" #include "QtConcurrentRun"
#include "geometry.h"
#include "nemo_interface/SnakeTile.h" #include "nemo_interface/SnakeTile.h"
#include "snake.h"
#include <ctime> #include <ctime>
#include <boost/units/systems/si.hpp> #include <boost/units/systems/si.hpp>
...@@ -15,6 +15,14 @@ ...@@ -15,6 +15,14 @@
#define SNAKE_MAX_TILES 1000 #define SNAKE_MAX_TILES 1000
#endif #endif
using namespace geometry;
namespace trans = bg::strategy::transform;
// Aux function
bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles,
BoundingBox &bbox);
QGC_LOGGING_CATEGORY(MeasurementAreaLog, "MeasurementAreaLog") QGC_LOGGING_CATEGORY(MeasurementAreaLog, "MeasurementAreaLog")
namespace { namespace {
...@@ -460,7 +468,7 @@ void MeasurementArea::resetProgress() { ...@@ -460,7 +468,7 @@ void MeasurementArea::resetProgress() {
//! \pre MeasurementArea::deferUpdate must be called first, don't call //! \pre MeasurementArea::deferUpdate must be called first, don't call
//! this function directly! //! this function directly!
void MeasurementArea::doUpdate() { void MeasurementArea::doUpdate() {
using namespace snake; using namespace geometry;
using namespace boost::units; using namespace boost::units;
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
...@@ -493,10 +501,8 @@ void MeasurementArea::doUpdate() { ...@@ -493,10 +501,8 @@ void MeasurementArea::doUpdate() {
areaToEnu(origin, polygon, polygonENU); areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU; std::vector<FPolygon> tilesENU;
BoundingBox bbox; BoundingBox bbox;
std::string errorString;
// Generate tiles. // Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox, if (getTiles(polygonENU, height, width, minArea, tilesENU, bbox)) {
errorString)) {
// Convert to geo system. // Convert to geo system.
for (const auto &t : tilesENU) { for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get()); auto geoTile = new SnakeTile(pData.get());
...@@ -507,8 +513,8 @@ void MeasurementArea::doUpdate() { ...@@ -507,8 +513,8 @@ void MeasurementArea::doUpdate() {
} }
pData->tiles.append(geoTile); pData->tiles.append(geoTile);
// Calculate center. // Calculate center.
snake::FPoint center; geometry::FPoint center;
snake::polygonCenter(t, center); geometry::polygonCenter(t, center);
QGeoCoordinate geoCenter; QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter); fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter)); pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
...@@ -625,3 +631,106 @@ void MeasurementArea::setHoldProgress(bool holdProgress) { ...@@ -625,3 +631,106 @@ void MeasurementArea::setHoldProgress(bool holdProgress) {
emit holdProgressChanged(); emit holdProgressChanged();
} }
} }
bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &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<boost::geometry::degree, double, 2, 2> rotate(
bbox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(),
-origin.get<1>());
FPolygon translated_polygon;
FPolygon rotated_polygon;
boost::geometry::transform(area, translated_polygon, translate);
boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
bg::correct(rotated_polygon);
// cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t iMax = ceil(bboxWidth / tileWidth.value());
size_t jMax = ceil(bboxHeight / tileHeight.value());
if (iMax < 1 || jMax < 1) {
std::stringstream ss;
ss << "Tile width (" << tileWidth << ") or tile height (" << tileHeight
<< ") to large for measurement area.";
qCDebug(MeasurementAreaLog) << ss.str().c_str();
return false;
}
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(
-bbox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(),
origin.get<1>());
for (size_t i = 0; i < iMax; ++i) {
double x_min = tileWidth.value() * i;
double x_max = x_min + tileWidth.value();
for (size_t j = 0; j < jMax; ++j) {
double y_min = tileHeight.value() * j;
double y_max = y_min + tileHeight.value();
FPolygon tile_unclipped;
tile_unclipped.outer().push_back(FPoint{x_min, y_min});
tile_unclipped.outer().push_back(FPoint{x_min, y_max});
tile_unclipped.outer().push_back(FPoint{x_max, y_max});
tile_unclipped.outer().push_back(FPoint{x_max, y_min});
tile_unclipped.outer().push_back(FPoint{x_min, y_min});
std::deque<FPolygon> boost_tiles;
if (!boost::geometry::intersection(tile_unclipped, rotated_polygon,
boost_tiles))
continue;
for (FPolygon t : boost_tiles) {
if (bg::area(t) > minTileArea.value()) {
// Transform boost_tile to world coordinate system.
FPolygon rotated_tile;
FPolygon translated_tile;
boost::geometry::transform(t, rotated_tile, rotate_back);
boost::geometry::transform(rotated_tile, translated_tile,
translate_back);
// Store tile and calculate center point.
tiles.push_back(translated_tile);
}
}
}
}
if (tiles.size() < 1) {
std::stringstream ss;
ss << "No tiles calculated. Is the minTileArea (" << minTileArea
<< ") parameter large enough?";
qCDebug(MeasurementAreaLog) << ss.str().c_str();
return false;
}
return true;
}
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include <array> #include <array>
#include <atomic> #include <atomic>
#include <functional> #include <functional>
#include <iostream>
#include <memory> #include <memory>
#include <string> #include <string>
#include <vector> #include <vector>
...@@ -23,26 +24,25 @@ namespace bu = boost::units; ...@@ -23,26 +24,25 @@ namespace bu = boost::units;
#include "QGCQGeoCoordinate.h" #include "QGCQGeoCoordinate.h"
#include "QmlObjectListModel.h" #include "QmlObjectListModel.h"
using namespace std; namespace geometry {
namespace snake {
//========================================================================= //=========================================================================
// Geometry stuff. // Geometry stuff.
//========================================================================= //=========================================================================
// Double geometry. // Double geometry.
using FloatType = double; typedef double FloatType;
typedef bg::model::point<double, 2, bg::cs::cartesian> FPoint; typedef bg::model::point<double, 2, bg::cs::cartesian> FPoint;
typedef bg::model::polygon<FPoint> FPolygon; typedef bg::model::polygon<FPoint> FPolygon;
typedef bg::model::linestring<FPoint> FLineString; typedef bg::model::linestring<FPoint> FLineString;
typedef bg::model::box<FPoint> FBox; typedef bg::model::box<FPoint> FBox;
typedef std::vector<FLineString> LineStringArray;
// Integer geometry. // Integer geometry.
using IntType = long long; typedef long long IntType;
using IPoint = bg::model::point<IntType, 2, bg::cs::cartesian>; typedef bg::model::point<IntType, 2, bg::cs::cartesian> IPoint;
using IPolygon = bg::model::polygon<IPoint>; typedef bg::model::polygon<IPoint> IPolygon;
using IRing = bg::model::ring<IPoint>; typedef bg::model::ring<IPoint> IRing;
using ILineString = bg::model::linestring<IPoint>; typedef bg::model::linestring<IPoint> ILineString;
FPoint int2Float(const IPoint &ip); FPoint int2Float(const IPoint &ip);
FPoint int2Float(const IPoint &ip, IntType scale); FPoint int2Float(const IPoint &ip, IntType scale);
...@@ -52,7 +52,7 @@ IPoint float2Int(const FPoint &ip, IntType scale); ...@@ -52,7 +52,7 @@ IPoint float2Int(const FPoint &ip, IntType scale);
template <class T> class Matrix; template <class T> class Matrix;
template <class DataType> template <class DataType>
ostream &operator<<(ostream &os, const Matrix<DataType> &matrix) { std::ostream &operator<<(std::ostream &os, const Matrix<DataType> &matrix) {
for (std::size_t i = 0; i < matrix.m(); ++i) { for (std::size_t i = 0; i < matrix.m(); ++i) {
for (std::size_t j = 0; j < matrix.n(); ++j) { for (std::size_t j = 0; j < matrix.n(); ++j) {
os << "(" << i << "," << j << "):" << matrix(i, j) << std::endl; os << "(" << i << "," << j << "):" << matrix(i, j) << std::endl;
...@@ -85,7 +85,8 @@ public: ...@@ -85,7 +85,8 @@ public:
std::size_t m() const { return _n; } std::size_t m() const { return _n; }
std::size_t n() const { return _n; } std::size_t n() const { return _n; }
friend ostream &operator<<<>(ostream &os, const Matrix<DataType> &dt); friend std::ostream &operator<<<>(std::ostream &os,
const Matrix<DataType> &dt);
private: private:
std::vector<DataType> _matrix; std::vector<DataType> _matrix;
...@@ -104,46 +105,100 @@ struct BoundingBox { ...@@ -104,46 +105,100 @@ struct BoundingBox {
FPolygon corners; FPolygon corners;
}; };
constexpr int earth_radius = 6371000; // meters (m)
constexpr double epsilon = std::numeric_limits<double>::epsilon(); // meters (m)
template <class GeoPoint1, class GeoPoint2> template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) { 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; double lat_rad = in.latitude() * M_PI / 180;
proj.Forward(in.latitude(), in.longitude(), 0, x, y, z); 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<0>(x);
out.set<1>(y); out.set<1>(y);
(void)z;
} }
template <class GeoPoint1, class GeoPoint2, class Point> template <class GeoPoint1, class GeoPoint2, class Point>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) { 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; using namespace std;
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 = 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.setX(x);
out.setY(y); out.setY(y);
(void)z;
} }
template <class GeoPoint> template <class GeoPoint>
void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) { void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f()); using namespace std;
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0,
earth); double x_rad = in.get<0>() / earth_radius;
double y_rad = in.get<1>() / earth_radius;
double lat = 0, lon = 0, alt = 0; double c = sqrt(y_rad * y_rad + x_rad * x_rad);
proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt); double sin_c = sin(c);
out.setLatitude(lat); double cos_c = cos(c);
out.setLongitude(lon);
out.setAltitude(alt); 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 <class GeoPoint, class Container1, class Container2> template <class GeoPoint, class Container1, class Container2>
...@@ -225,59 +280,17 @@ bool joinedArea(const std::vector<FPolygon *> &areas, FPolygon &jArea); ...@@ -225,59 +280,17 @@ bool joinedArea(const std::vector<FPolygon *> &areas, FPolygon &jArea);
bool joinedArea(const FPolygon &mArea, const FPolygon &sArea, bool joinedArea(const FPolygon &mArea, const FPolygon &sArea,
const FPolygon &corridor, FPolygon &jArea, const FPolygon &corridor, FPolygon &jArea,
std::string &errorString); std::string &errorString);
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth, } // namespace geometry
Area minTileArea, std::vector<FPolygon> &tiles, BoundingBox &bbox,
std::string &errorString);
using Transects = vector<FLineString>;
using Progress = vector<int>;
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const FPolygon &mArea,
const std::vector<FPolygon> &tiles,
const Progress &p, Transects &t,
string &errorString);
struct TransectInfo {
TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
size_t index;
bool reversed;
};
struct Route {
FLineString path;
std::vector<TransectInfo> info;
};
using Solution =
std::vector<Route>; // Every route corresponds to one run/vehicle
struct RouteParameter {
RouteParameter()
: numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5),
stop([] { return false; }) {}
std::size_t numSolutionsPerRun;
std::size_t numRuns;
std::size_t minNumTransectsPerRun;
std::function<bool(void)> stop;
mutable std::string errorString;
};
bool route(const FPolygon &area, const Transects &transects,
std::vector<Solution> &solutionVector,
const RouteParameter &par = RouteParameter());
namespace detail {
const double offsetConstant =
0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
} // namespace snake
// operator== and operator!= for boost point // operator== and operator!= for boost point
namespace boost { namespace boost {
namespace geometry { namespace geometry {
namespace model { namespace model {
bool operator==(snake::FPoint &p1, snake::FPoint &p2); bool operator==(::geometry::FPoint &p1, ::geometry::FPoint &p2);
bool operator!=(snake::FPoint &p1, snake::FPoint &p2); bool operator!=(::geometry::FPoint &p1, ::geometry::FPoint &p2);
bool operator==(snake::IPoint &p1, snake::IPoint &p2); bool operator==(::geometry::IPoint &p1, ::geometry::IPoint &p2);
bool operator!=(snake::IPoint &p1, snake::IPoint &p2); bool operator!=(::geometry::IPoint &p1, ::geometry::IPoint &p2);
} // namespace model } // namespace model
} // namespace geometry } // namespace geometry
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment