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/Snake/snake.h

@@ -27,59 +27,68 @@ namespace snake {
 // Geometry stuff.
 //=========================================================================
 
-typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
-// typedef std::vector<BoostPoint>                        BoostPointList;
-typedef bg::model::polygon<BoostPoint> BoostPolygon;
-typedef bg::model::linestring<BoostPoint> BoostLineString;
-typedef std::vector<std::vector<int64_t>> Int64Matrix;
-typedef bg::model::box<BoostPoint> BoostBox;
-
-template <class T> class Matrix {
+// Double geometry.
+using FloatType = double;
+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;
+
+// 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>;
+
+FPoint int2Float(const IPoint &ip);
+FPoint int2Float(const IPoint &ip, IntType scale);
+IPoint float2Int(const FPoint &ip);
+IPoint float2Int(const FPoint &ip, IntType scale);
+
+template <class T> class Matrix;
+
+template <class DataType>
+ostream &operator<<(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;
+    }
+  }
+  return os;
+}
+
+// Matrix
+template <class DataType> class Matrix {
 public:
-  Matrix() : _elements(0), _m(0), _n(0), _isInitialized(false) {}
-  Matrix(size_t m, size_t n) : Matrix(m, n, T{0}) {}
-  Matrix(size_t m, size_t n, T value)
-      : _elements(m * n), _m(m), _n(n), _isInitialized(true) {
-    assert((m > 0) || (n > 0));
-    _matrix.resize(_elements, value);
+  using value_type = DataType;
+  Matrix(std::size_t m, std::size_t n) : _m(m), _n(n) { _matrix.resize(m * n); }
+  Matrix(std::size_t m, std::size_t n, DataType value) : _m(m), _n(n) {
+    _matrix.resize(m * n, value);
   }
 
-  double get(size_t i, size_t j) const {
-    assert(_isInitialized);
+  DataType &operator()(std::size_t i, std::size_t j) {
     assert(i < _m);
     assert(j < _n);
     return _matrix[i * _m + j];
   }
 
-  size_t getM() const { return _m; }
-  size_t getN() const { return _n; }
-
-  void set(size_t i, size_t j, const T &value) {
-    assert(_isInitialized);
+  const DataType &operator()(std::size_t i, std::size_t j) const {
     assert(i < _m);
     assert(j < _n);
-    _matrix[i * _m + j] = value;
+    return _matrix[i * _m + j];
   }
 
-  void setDimension(size_t m, size_t n, const T &value) {
-    assert((m > 0) || (n > 0));
-    assert(!_isInitialized);
-    _m = m;
-    _n = n;
-    _elements = n * m;
-    _matrix.resize(_elements, value);
-    _isInitialized = true;
-  }
+  std::size_t m() const { return _n; }
+  std::size_t n() const { return _n; }
 
-  void setDimension(size_t m, size_t n) { setDimension(m, n, T{0}); }
+  friend ostream &operator<<<>(ostream &os, const Matrix<DataType> &dt);
 
 private:
-  size_t _elements;
-  size_t _m;
-  size_t _n;
-  bool _isInitialized;
-  std::vector<T> _matrix;
-};
+  std::vector<DataType> _matrix;
+  const std::size_t _m;
+  const std::size_t _n;
+}; // Matrix
 
 struct BoundingBox {
   BoundingBox();
@@ -89,11 +98,11 @@ struct BoundingBox {
   double width;
   double height;
   double angle;
-  BoostPolygon corners;
+  FPolygon corners;
 };
 
 template <class GeoPoint1, class GeoPoint2>
-void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, BoostPoint &out) {
+void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
   static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                          GeographicLib::Constants::WGS84_f());
   GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
@@ -125,7 +134,7 @@ void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
 }
 
 template <class GeoPoint>
-void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
+void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
   static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                          GeographicLib::Constants::WGS84_f());
   GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
@@ -148,9 +157,9 @@ void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) {
 }
 
 template <class GeoPoint, class Container>
-void areaToEnu(const GeoPoint &origin, const Container &in, BoostPolygon &out) {
+void areaToEnu(const GeoPoint &origin, const Container &in, FPolygon &out) {
   for (auto &vertex : in) {
-    BoostPoint p;
+    FPoint p;
     toENU(origin, vertex, p);
     out.outer().push_back(p);
   }
@@ -168,8 +177,7 @@ void areaFromEnu(const GeoPoint &origin, Container1 &in,
 }
 
 template <class GeoPoint, class Container>
-void areaFromEnu(const GeoPoint &origin, BoostPolygon &in,
-                 const Container &out) {
+void areaFromEnu(const GeoPoint &origin, FPolygon &in, const Container &out) {
   for (auto &vertex : in.outer()) {
     typename Container::value_type p;
     fromENU(origin, vertex, p);
@@ -177,19 +185,19 @@ void areaFromEnu(const GeoPoint &origin, BoostPolygon &in,
   }
 }
 
-void polygonCenter(const BoostPolygon &polygon, BoostPoint &center);
-bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox);
-void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset,
+void polygonCenter(const FPolygon &polygon, FPoint &center);
+bool minimalBoundingBox(const FPolygon &polygon, BoundingBox &minBBox);
+void offsetPolygon(const FPolygon &polygon, FPolygon &polygonOffset,
                    double offset);
-void graphFromPolygon(const BoostPolygon &polygon,
-                      const BoostLineString &vertices, Matrix<double> &graph);
+void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices,
+                      Matrix<double> &graph);
 bool toDistanceMatrix(Matrix<double> &graph);
-bool dijkstraAlgorithm(
-    const size_t numElements, size_t startIndex, size_t endIndex,
-    std::vector<size_t> &elementPath,
-    std::function<double(const size_t, const size_t)> distanceDij);
-void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
-                           size_t endIndex, std::vector<size_t> &pathIdx);
+bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex,
+                       std::vector<size_t> &elementPath, double &length,
+                       std::function<double(size_t, size_t)> distanceDij);
+
+bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
+                           const size_t endIndex, std::vector<size_t> &pathIdx);
 
 typedef bu::quantity<bu::si::length> Length;
 typedef bu::quantity<bu::si::area> Area;
@@ -197,21 +205,21 @@ typedef bu::quantity<bu::si::plane_angle> Angle;
 typedef bu::quantity<bu::si::plane_angle> Radian;
 typedef bu::quantity<bu::degree::plane_angle> Degree;
 
-bool joinedArea(const std::vector<BoostPolygon *> &areas, BoostPolygon &jArea);
-bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea,
-                const BoostPolygon &corridor, BoostPolygon &jArea,
+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);
-bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth,
-           Area minTileArea, std::vector<BoostPolygon> &tiles,
-           BoundingBox &bbox, std::string &errorString);
 
-using Transects = vector<BoostLineString>;
+using Transects = vector<FLineString>;
 using Progress = vector<int>;
-using Route = BoostLineString;
+using Route = FLineString;
 
 bool transectsFromScenario(Length distance, Length minLength, Angle angle,
-                           const BoostPolygon &mArea,
-                           const std::vector<BoostPolygon> &tiles,
+                           const FPolygon &mArea,
+                           const std::vector<FPolygon> &tiles,
                            const Progress &p, Transects &t,
                            string &errorString);
 
@@ -220,10 +228,17 @@ struct TransectInfo {
   size_t index;
   bool reversed;
 };
-bool route(const BoostPolygon &area, const Transects &transects,
+bool route(const FPolygon &area, const Transects &transects,
+           std::vector<TransectInfo> &transectInfo, Route &r,
+           string &errorString);
+bool route(const FPolygon &area, const Transects &transects,
+           std::vector<TransectInfo> &transectInfo, Route &r,
+           std::function<bool(void)> stop, string &errorString);
+
+bool route_old(const FPolygon &area, const Transects &transects,
                std::vector<TransectInfo> &transectInfo, Route &r,
                string &errorString);
-bool route(const BoostPolygon &area, const Transects &transects,
+bool route_old(const FPolygon &area, const Transects &transects,
                std::vector<TransectInfo> &transectInfo, Route &r,
                std::function<bool(void)> stop, string &errorString);
 
@@ -238,8 +253,8 @@ namespace boost {
 namespace geometry {
 namespace model {
 
-bool operator==(snake::BoostPoint p1, snake::BoostPoint p2);
-bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2);
+bool operator==(snake::FPoint p1, snake::FPoint p2);
+bool operator!=(snake::FPoint p1, snake::FPoint p2);
 
 } // namespace model
 } // namespace geometry