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 {
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++ {
......
......@@ -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 \
......
......@@ -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<snake::FPolygon> outputENU;
std::deque<geometry::FPolygon> 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);
}
}
......
......@@ -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();
......
......@@ -7,7 +7,7 @@
#include <functional>
#include <memory>
#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<bool(snake::Transects &)>;
using Work = std::function<bool(geometry::LineStringArray &)>;
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);
......
......@@ -23,10 +23,11 @@ const char *minLengthKey = "MinLength";
QGC_LOGGING_CATEGORY(LinearGeneratorLog, "LinearGeneratorLog")
bool linearTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length distance, snake::Angle angle,
snake::Length minLength, snake::Transects &transects);
bool linearTransects(const geometry::FPolygon &polygon,
const std::vector<geometry::FPolygon> &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::FPolygon>();
snake::areaToEnu(origin, geoPolygon, *pPolygon);
auto pPolygon = std::make_shared<geometry::FPolygon>();
geometry::areaToEnu(origin, geoPolygon, *pPolygon);
// Progress and tiles.
const auto &progress = measurementArea->progress();
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()) {
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<const SnakeTile *>(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<snake::FPolygon> &tiles,
snake::Length distance, snake::Angle angle,
snake::Length minLength, snake::Transects &transects) {
bool linearTransects(const geometry::FPolygon &polygon,
const std::vector<geometry::FPolygon> &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<bg::degree, double, 2, 2> 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<ClipperLib::Path> transectsClipper;
std::vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i = 0; i < num_t; ++i) {
// calculate transect
snake::FPoint v1{x0, y0 + i * distance.value()};
snake::FPoint v2{x1, y0 + i * distance.value()};
snake::FLineString transect;
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<bg::degree, double, 2, 2> 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<ClipperLib::Path> processedTiles;
std::vector<ClipperLib::Path> 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<double>(clipperTransect[0].X) / 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].Y) / CLIPPER_SCALE};
snake::FLineString transect{v1, v2};
geometry::FLineString transect{v1, v2};
if (bg::length(transect) >= minLength.value()) {
transects.push_back(transect);
}
......
......@@ -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.
......
......@@ -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<QGCQGeoCoordinate *>(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<QGeoCoordinate>(), vertexENU);
geometry::FPoint vertexENU;
geometry::toENU(origin, vertex.value<QGeoCoordinate>(), 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<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) {
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 {
......
......@@ -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<RoutingData>;
using PtrRoutingData = std::shared_ptr<RoutingResult>;
using PtrWorker = RoutingThread *;
using Variant = QVariantList;
......
......@@ -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<const SnakeTile *>(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.";
......
......@@ -4,24 +4,40 @@
#include <QSharedPointer>
#include <QThread>
#include "geometry/snake.h"
#include "geometry/geometry.h"
#include <atomic>
#include <condition_variable>
#include <functional>
#include <mutex>
struct RoutingData {
snake::Transects transects;
std::vector<snake::Solution> 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<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;
};
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<std::mutex>;
public:
using PtrRoutingData = shared_ptr<RoutingData>;
using Generator = std::function<bool(snake::Transects &)>;
using Consumer = std::function<void(const RoutingData &)>;
using PtrRoutingData = std::shared_ptr<RoutingResult>;
using Work = std::function<bool(geometry::LineStringArray &)>;
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;
......
#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<QGCQGeoCoordinate *>(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<QGCQGeoCoordinate *>(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();
}
......
#include "MeasurementArea.h"
#include "QtConcurrentRun"
#include "geometry.h"
#include "nemo_interface/SnakeTile.h"
#include "snake.h"
#include <ctime>
#include <boost/units/systems/si.hpp>
......@@ -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<FPolygon> &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<FPolygon> 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<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 @@
#include <array>
#include <atomic>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
......@@ -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<double, 2, bg::cs::cartesian> FPoint;
typedef bg::model::polygon<FPoint> FPolygon;
typedef bg::model::linestring<FPoint> FLineString;
typedef bg::model::box<FPoint> FBox;
typedef std::vector<FLineString> LineStringArray;
// Integer geometry.
using IntType = long long;
using IPoint = bg::model::point<IntType, 2, bg::cs::cartesian>;
using IPolygon = bg::model::polygon<IPoint>;
using IRing = bg::model::ring<IPoint>;
using ILineString = bg::model::linestring<IPoint>;
typedef long long IntType;
typedef bg::model::point<IntType, 2, bg::cs::cartesian> IPoint;
typedef bg::model::polygon<IPoint> IPolygon;
typedef bg::model::ring<IPoint> IRing;
typedef bg::model::linestring<IPoint> 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 T> class Matrix;
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 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<DataType> &dt);
friend std::ostream &operator<<<>(std::ostream &os,
const Matrix<DataType> &dt);
private:
std::vector<DataType> _matrix;
......@@ -104,46 +105,100 @@ struct BoundingBox {
FPolygon corners;
};
constexpr int earth_radius = 6371000; // meters (m)
constexpr double epsilon = std::numeric_limits<double>::epsilon(); // meters (m)
template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 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 <class GeoPoint1, class GeoPoint2, class Point>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 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 <class GeoPoint>
void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 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 <class GeoPoint, class Container1, class Container2>
......@@ -225,59 +280,17 @@ bool joinedArea(const std::vector<FPolygon *> &areas, FPolygon &jArea);
bool joinedArea(const FPolygon &mArea, const FPolygon &sArea,
const FPolygon &corridor, FPolygon &jArea,
std::string &errorString);
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles, BoundingBox &bbox,
std::string &errorString);
using Transects = vector<FLineString>;
using Progress = vector<int>;
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const FPolygon &mArea,
const std::vector<FPolygon> &tiles,
const Progress &p, Transects &t,
string &errorString);
struct TransectInfo {
TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
size_t index;
bool reversed;
};
struct Route {
FLineString path;
std::vector<TransectInfo> info;
};
using Solution =
std::vector<Route>; // Every route corresponds to one run/vehicle
struct RouteParameter {
RouteParameter()
: numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5),
stop([] { return false; }) {}
std::size_t numSolutionsPerRun;
std::size_t numRuns;
std::size_t minNumTransectsPerRun;
std::function<bool(void)> stop;
mutable std::string errorString;
};
bool route(const FPolygon &area, const Transects &transects,
std::vector<Solution> &solutionVector,
const RouteParameter &par = RouteParameter());
namespace detail {
const double offsetConstant =
0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
} // namespace snake
} // 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
......
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