snake::route() now a lot faster

qgroundcontrol.pro

@@ -28,8 +28,8 @@ QGCROOT  = $$PWD
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
     DEFINES += DEBUG
-    DEFINES += SNAKE_SHOW_TIME
-    DEFINES += DEBUG_SRTL
+    #DEFINES += SNAKE_SHOW_TIME
+    #DEFINES += DEBUG_SRTL
     #DEFINES += SNAKE_DEBUG
     DEFINES += SHOW_CIRCULAR_SURVEY_TIME
     DEFINES += DEBUG_CIRCULAR_SURVEY
@@ -38,9 +38,10 @@ DebugBuild {
 else {
     DESTDIR  = $${OUT_PWD}/release
     #DEFINES += ROS_BRIDGE_DEBUG
-    #DEFINES += SHOW_CIRCULAR_SURVEY_TIME
-    #DEFINES += SNAKE_SHOW_TIME
-    DEFINES += DEBUG_SRTL
+    DEFINES += SHOW_CIRCULAR_SURVEY_TIME
+    DEFINES += SNAKE_SHOW_TIME
+    #DEFINES += SNAKE_DEBUG
+    #DEFINES += DEBUG_SRTL
     DEFINES += NDEBUG
 }
 

src/Wima/Geometry/WimaMeasurementArea.cc

@@ -259,9 +259,9 @@ void WimaMeasurementArea::doUpdate() {
       DataPtr pData(new TileData());
       // Convert to ENU system.
       QGeoCoordinate origin = polygon.first();
-      BoostPolygon polygonENU;
+      FPolygon polygonENU;
       areaToEnu(origin, polygon, polygonENU);
-      std::vector<BoostPolygon> tilesENU;
+      std::vector<FPolygon> tilesENU;
       BoundingBox bbox;
       std::string errorString;
       // Generate tiles.
@@ -277,7 +277,7 @@ void WimaMeasurementArea::doUpdate() {
           }
           pData->tiles.append(geoTile);
           // Calculate center.
-          snake::BoostPoint center;
+          snake::FPoint center;
           snake::polygonCenter(t, center);
           QGeoCoordinate geoCenter;
           fromENU(origin, center, geoCenter);

src/Wima/RoutingThread.cpp

@@ -23,7 +23,7 @@ RoutingThread::~RoutingThread() {
 
 bool RoutingThread::calculating() const { return this->_calculating; }
 
-void RoutingThread::route(const snake::BoostPolygon &safeArea,
+void RoutingThread::route(const snake::FPolygon &safeArea,
                           const RoutingThread::Generator &generator) {
   // Sample input.
   Lock lk(this->_mutex);
@@ -81,9 +81,37 @@ void RoutingThread::run() {
           return restart || expired;
         };
         std::string errorString;
-        // Route transects;
+        // Route transects
+        //#ifdef SHOW_CIRCULAR_SURVEY_TIME
+        //        auto s = std::chrono::high_resolution_clock::now();
+        //#endif
+        //        snake::route_old(safeAreaENU, transectsENU, transectsInfo,
+        //        route,
+        //                         stopLambda, errorString);
+        //#ifdef SHOW_CIRCULAR_SURVEY_TIME
+        //        qWarning() << "RoutingWorker::run(): route_old time: "
+        //                   <<
+        //                   std::chrono::duration_cast<std::chrono::milliseconds>(
+        //                          std::chrono::high_resolution_clock::now() -
+        //                          s) .count()
+        //                   << " ms";
+        //#endif
+        //#ifdef SHOW_CIRCULAR_SURVEY_TIME
+        //        s = std::chrono::high_resolution_clock::now();
+        //#endif
+        //        transectsInfo.clear();
+        //        route.clear();
+        //        errorString.clear();
         bool success = snake::route(safeAreaENU, transectsENU, transectsInfo,
                                     route, stopLambda, errorString);
+        //#ifdef SHOW_CIRCULAR_SURVEY_TIME
+        //        qWarning() << "RoutingWorker::run(): route time: "
+        //                   <<
+        //                   std::chrono::duration_cast<std::chrono::milliseconds>(
+        //                          std::chrono::high_resolution_clock::now() -
+        //                          s) .count()
+        //                   << " ms";
+        //#endif
         // Check if routing was successful.
         if ((!success || route.size() < 3) && !this->_restart) {
 #ifdef DEBUG_CIRCULAR_SURVEY

src/Wima/RoutingThread.h

@@ -11,7 +11,7 @@
 #include <mutex>
 
 struct RoutingData {
-  snake::BoostLineString route;
+  snake::FLineString route;
   snake::Transects transects;
   std::vector<snake::TransectInfo> transectsInfo;
 };
@@ -35,7 +35,7 @@ public:
   bool calculating() const;
 
 public slots:
-  void route(const snake::BoostPolygon &safeArea, const Generator &generator);
+  void route(const snake::FPolygon &safeArea, const Generator &generator);
 
 signals:
   void result(PtrRoutingData pTransects);
@@ -48,7 +48,7 @@ private:
   mutable std::mutex _mutex;
   mutable std::condition_variable _cv;
   // Internal data
-  snake::BoostPolygon _safeArea;
+  snake::FPolygon _safeArea;
   Generator _generator; // transect generator
   // State
   std::atomic_bool _calculating;

src/Wima/WimaController.cc

@@ -18,7 +18,7 @@
 #include "QVector3D"
 #include <QScopedPointer>
 
-#define CLIPPER_SCALE 10000
+#define CLIPPER_SCALE 1000000
 #include "clipper/clipper.hpp"
 
 #include <memory>
@@ -906,7 +906,7 @@ void WimaController::_updateRoute() {
       this->_origin = this->_joinedArea.coordinateList().first();
       this->_origin.setAltitude(0);
       const auto &origin = this->_origin;
-      auto pTiles = std::make_shared<std::vector<snake::BoostPolygon>>();
+      auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
       std::size_t numTiles = 0;
       for (int i = 0; i < progress.size(); ++i) {
         if (progress[i] == 100) {
@@ -914,7 +914,7 @@ void WimaController::_updateRoute() {
           const auto *obj = tiles->get(i);
           const auto *tile = qobject_cast<const SnakeTile *>(obj);
           if (tile != nullptr) {
-            snake::BoostPolygon t;
+            snake::FPolygon t;
             snake::areaToEnu(origin, tile->coordinateList(), t);
             pTiles->push_back(std::move(t));
           } else {
@@ -928,18 +928,18 @@ void WimaController::_updateRoute() {
         for (auto &v : safeArea) {
           v.setAltitude(0);
         }
-        snake::BoostPolygon safeAreaENU;
+        snake::FPolygon safeAreaENU;
         snake::areaToEnu(origin, safeArea, safeAreaENU);
         const auto &depot = this->_serviceArea.depot();
-        snake::BoostPoint depotENU;
+        snake::FPoint depotENU;
         snake::toENU(origin, depot, depotENU);
         // Fetch geo transects and convert to ENU.
         const auto &geoTransects = this->_rawTransects;
         auto pUnclippedTransects = std::make_shared<snake::Transects>();
         for (auto &geoTransect : geoTransects) {
-          snake::BoostLineString t;
+          snake::FLineString t;
           for (auto &geoVertex : geoTransect) {
-            snake::BoostPoint v;
+            snake::FPoint v;
             snake::toENU(origin, geoVertex, v);
             t.push_back(v);
           }
@@ -983,11 +983,11 @@ void WimaController::_updateRoute() {
                           ClipperLib::pftNonZero, ClipperLib::pftNonZero);
           // Extract transects from  PolyTree and convert them to
           // BoostLineString
-          transects.push_back(snake::BoostLineString{depotENU});
+          transects.push_back(snake::FLineString{depotENU});
           for (const auto &child : clippedTransecs.Childs) {
-            snake::BoostLineString transect;
+            snake::FLineString transect;
             for (const auto &v : child->Contour) {
-              snake::BoostPoint c{static_cast<double>(v.X) / CLIPPER_SCALE,
+              snake::FPoint c{static_cast<double>(v.X) / CLIPPER_SCALE,
                                   static_cast<double>(v.Y) / CLIPPER_SCALE};
               transect.push_back(c);
             }