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

libGeographic dependency removed, snake namespace renamed to geometry, geometry.h and geometry.cpp cleared up

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++ {

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 \

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<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);
         }
       }

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<snake::FPolygon> &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<geometry::FPolygon> &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<const MeasurementArea *>(*this->_d->areaList());
@@ -114,13 +115,13 @@ bool CircularGenerator::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) {
@@ -128,8 +129,8 @@ bool CircularGenerator::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(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<snake::FPolygon> &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<geometry::FPolygon> &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<snake::Length> distances;
+      std::vector<geometry::Length> distances;
       distances.reserve(polygon.outer().size());
-      std::vector<snake::Angle> angles;
+      std::vector<geometry::Angle> 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<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
+      std::vector<ClipperLib::Path> 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<ClipperLib::Path> processedTiles;
+        std::vector<ClipperLib::Path> 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<double>(vertex.X) / CLIPPER_SCALE;
           auto y = static_cast<double>(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);

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();

src/MeasurementComplexItem/GeneratorBase.h

@@ -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);

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<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);
         }

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.

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<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 {

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<RoutingData>;
+  using PtrRoutingData = std::shared_ptr<RoutingResult>;
   using PtrWorker = RoutingThread *;
   using Variant = QVariantList;
 

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<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.";

src/MeasurementComplexItem/RoutingThread.cpp

@@ -3,10 +3,35 @@
 #include <chrono>
 // Qt
 #include <QDebug>
-
+// 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<bool(void)> stop;
+  mutable std::string errorString;
+};
+
+bool getRoute(const FPolygon &area, const LineStringArray &transects,
+              std::vector<Solution> &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<Solution> &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<IPoint> vertices;
+  std::vector<std::pair<std::size_t, std::size_t>> disjointNodes;
+  std::vector<VirtualNode> nodeList;
+  std::vector<NodeToTransect> 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<double> 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<double>::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<IntType> 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<std::size_t> 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<IntType> 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<IntType>::max();
+    routingMatrix(j, i) = std::numeric_limits<IntType>::max();
+  }
+#ifdef SNAKE_DEBUG
+  std::cout << "routing matrix:" << std::endl;
+  std::cout << routingMatrix << std::endl;
+#endif
+
+  // Create Routing Index Manager.
+  auto minNumTransectsPerRun =
+      std::max<std::size_t>(1, par.minNumTransectsPerRun);
+  auto maxRuns = std::max<std::size_t>(
+      1, std::floor(double(transects.size()) / minNumTransectsPerRun));
+  auto numRuns = std::max<std::size_t>(1, par.numRuns);
+  numRuns = std::min<std::size_t>(numRuns, maxRuns);
+
+  RoutingIndexManager::NodeIndex depot(0);
+  //  std::vector<RoutingIndexManager::NodeIndex> 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<int64> 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<std::size_t>(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::milliseconds>(
+      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<std::vector<const Assignment *>>();
+  (void)routing.SolveWithParameters(searchParameters, pSolutions.get());
+#ifdef SNAKE_SHOW_TIME
+  delta = std::chrono::duration_cast<std::chrono::milliseconds>(
+      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<size_t> 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<size_t> 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::milliseconds>(
+      std::chrono::high_resolution_clock::now() - start);
+  cout << "reconstruct route: " << delta.count() << " ms" << endl;
+#endif
+
+  if (solutionVector.size() > 0) {
+    return true;
+  } else {
+    return false;
+  }
+}

src/MeasurementComplexItem/RoutingThread.h

@@ -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;

src/MeasurementComplexItem/geometry/GeoArea.cc

 #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();
 }

src/MeasurementComplexItem/geometry/MeasurementArea.cc

 #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;
+}

src/MeasurementComplexItem/geometry/snake.cpp → src/MeasurementComplexItem/geometry/geometry.cpp

 #include <algorithm>
 #include <iostream>
 
-#include "snake.h"
+#include "geometry.h"
 
 #include <mapbox/geometry.hpp>
 #include <mapbox/polylabel.hpp>
@@ -14,13 +14,6 @@
 #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
@@ -30,7 +23,7 @@ namespace trans = bg::strategy::transform;
 
 BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(bg::cs::cartesian)
 
-namespace snake {
+namespace geometry {
 static const IntType stdScale = 1000000;
 //=========================================================================
 // Geometry stuff.
@@ -40,15 +33,15 @@ void polygonCenter(const FPolygon &polygon, FPoint &center) {
   using namespace mapbox;
   if (polygon.outer().empty())
     return;
-  geometry::polygon<double> p;
-  geometry::linear_ring<double> lr1;
+  mapbox::geometry::polygon<double> p;
+  mapbox::geometry::linear_ring<double> lr1;
   for (size_t i = 0; i < polygon.outer().size(); ++i) {
-    geometry::point<double> vertex(polygon.outer()[i].get<0>(),
-                                   polygon.outer()[i].get<1>());
+    mapbox::geometry::point<double> vertex(polygon.outer()[i].get<0>(),
+                                           polygon.outer()[i].get<1>());
     lr1.push_back(vertex);
   }
   p.push_back(lr1);
-  geometry::point<double> c = polylabel(p);
+  mapbox::geometry::point<double> c = polylabel(p);
 
   center.set<0>(c.x);
   center.set<1>(c.y);
@@ -270,109 +263,6 @@ bool toDistanceMatrix(Matrix<double> &graph) {
   return true;
 }
 
-bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
-           Area minTileArea, std::vector<FPolygon> &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<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.";
-    errorString = ss.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?";
-    errorString = ss.str();
-    return false;
-  }
-
-  return true;
-}
-
 bool joinedArea(const FPolygon &mArea, const FPolygon &sArea,
                 const FPolygon &corridor, FPolygon &jArea,
                 std::string &errorString) {
@@ -489,541 +379,6 @@ void BoundingBox::clear() {
   corners.clear();
 }
 
-bool transectsFromScenario(Length distance, Length minLength, Angle angle,
-                           const FPolygon &mArea,
-                           const std::vector<FPolygon> &tiles,
-                           const Progress &p, Transects &t,
-                           string &errorString) {
-  // Rotate measurement area by angle and calculate bounding box.
-  FPolygon mAreaRotated;
-  trans::rotate_transformer<bg::degree, double, 2, 2> 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<ClipperLib::Path> 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<bg::degree, double, 2, 2> rotate_back(
-        -angle.value() * 180 / M_PI);
-    bg::transform(transect, temp_transect, rotate_back);
-    // to clipper
-    ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
-                                temp_transect[0].get<0>() * CLIPPER_SCALE),
-                            static_cast<ClipperLib::cInt>(
-                                temp_transect[0].get<1>() * CLIPPER_SCALE)};
-    ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
-                                temp_transect[1].get<0>() * CLIPPER_SCALE),
-                            static_cast<ClipperLib::cInt>(
-                                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<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
-        static_cast<ClipperLib::cInt>(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<FPolygon> processedTiles;
-    for (size_t i = 0; i < numTiles; ++i) {
-      if (p[i] == 100) {
-        processedTiles.push_back(tiles[i]);
-      }
-    }
-
-    if (processedTiles.size() != numTiles) {
-      vector<ClipperLib::Path> processedTilesClipper;
-      for (const auto &t : processedTiles) {
-        ClipperLib::Path path;
-        for (const auto &vertex : t.outer()) {
-          path.push_back(ClipperLib::IntPoint{
-              static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
-              static_cast<ClipperLib::cInt>(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<double>(clipperTransect[0].X) / CLIPPER_SCALE,
-              static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
-    FPoint v2{static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
-              static_cast<double>(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<Solution> &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<IPoint> vertices;
-  std::vector<std::pair<std::size_t, std::size_t>> disjointNodes;
-  std::vector<VirtualNode> nodeList;
-  std::vector<NodeToTransect> 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<double> 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<double>::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<IntType> 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<std::size_t> 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<IntType> 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<IntType>::max();
-    routingMatrix(j, i) = std::numeric_limits<IntType>::max();
-  }
-#ifdef SNAKE_DEBUG
-  std::cout << "routing matrix:" << std::endl;
-  std::cout << routingMatrix << std::endl;
-#endif
-
-  // Create Routing Index Manager.
-  auto minNumTransectsPerRun =
-      std::max<std::size_t>(1, par.minNumTransectsPerRun);
-  auto maxRuns = std::max<std::size_t>(
-      1, std::floor(double(transects.size()) / minNumTransectsPerRun));
-  auto numRuns = std::max<std::size_t>(1, par.numRuns);
-  numRuns = std::min<std::size_t>(numRuns, maxRuns);
-
-  RoutingIndexManager::NodeIndex depot(0);
-  //  std::vector<RoutingIndexManager::NodeIndex> 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<int64> 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<std::size_t>(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::milliseconds>(
-      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<std::vector<const Assignment *>>();
-  (void)routing.SolveWithParameters(searchParameters, pSolutions.get());
-#ifdef SNAKE_SHOW_TIME
-  delta = std::chrono::duration_cast<std::chrono::milliseconds>(
-      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<size_t> 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<size_t> 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::milliseconds>(
-      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) {
@@ -1151,19 +506,23 @@ bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
   return true;
 }
 
-} // namespace snake
+} // namespace geometry
 
-bool boost::geometry::model::operator==(snake::FPoint &p1, snake::FPoint &p2) {
+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!=(snake::FPoint &p1, snake::FPoint &p2) {
+bool boost::geometry::model::operator!=(::geometry::FPoint &p1,
+                                        ::geometry::FPoint &p2) {
   return !(p1 == p2);
 }
 
-bool boost::geometry::model::operator==(snake::IPoint &p1, snake::IPoint &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!=(snake::IPoint &p1, snake::IPoint &p2) {
+bool boost::geometry::model::operator!=(::geometry::IPoint &p1,
+                                        ::geometry::IPoint &p2) {
   return !(p1 == p2);
 }

src/MeasurementComplexItem/geometry/snake.h → src/MeasurementComplexItem/geometry/geometry.h

@@ -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