temp

qgroundcontrol.pro

@@ -408,6 +408,7 @@ FORMS += \
 
 HEADERS += \
     src/Snake/clipper/clipper.hpp \
+    src/Snake/flight_plan.h \
     src/Snake/mapbox/feature.hpp \
     src/Snake/mapbox/geometry.hpp \
     src/Snake/mapbox/geometry/box.hpp \
@@ -430,22 +431,22 @@ HEADERS += \
     src/Snake/mapbox/variant_io.hpp \
     src/Snake/snake.h \
     src/Snake/snake_geometry.h \
-    src/Snake/snake_global.h \
+    src/Snake/snake_typedefs.h \
     src/Wima/Geometry/GeoPoint3D.h \
     src/Wima/Geometry/Polygon2D.h \
     src/Wima/Geometry/PolygonArray.h \
+    src/Wima/Snake/GeoPolygonArray.h \
+    src/Wima/Snake/GeoTile.h \
+    src/Wima/Snake/PolygonArray.h \
     src/Wima/Snake/QNemoHeartbeat.h \
     src/Wima/Snake/QNemoProgress.h \
     src/Wima/Snake/QtROSJsonFactory.h \
     src/Wima/Snake/QtROSTypeFactory.h \
-    src/Wima/Snake/SnakeTiles.h \
-    src/Wima/Snake/SnakeTilesLocal.h \
     src/Wima/Snake/SnakeWorker.h \
     src/Wima/WaypointManager/AreaInterface.h \
     src/Wima/WaypointManager/DefaultManager.h \
     src/Wima/WaypointManager/GenericWaypointManager.h \
     src/Wima/Geometry/WimaPolygonArray.h \
-    src/Wima/Snake/snaketile.h \
     src/Wima/WaypointManager/RTLManager.h \
     src/Wima/WaypointManager/Settings.h \
     src/Wima/WaypointManager/Slicer.h \
@@ -497,10 +498,12 @@ HEADERS += \
     src/comm/utilities.h
 SOURCES += \
     src/Snake/clipper/clipper.cpp \
+    src/Snake/flight_plan.cpp \
     src/Snake/snake.cpp \
     src/Snake/snake_geometry.cpp \
     src/Wima/Geometry/GeoPoint3D.cpp \
     src/Wima/Geometry/PolygonArray.cc \
+    src/Wima/Snake/GeoTile.cpp \
     src/Wima/Snake/QNemoProgress.cc \
     src/Wima/Snake/SnakeWorker.cc \
     src/Wima/WaypointManager/AreaInterface.cpp \
@@ -516,7 +519,6 @@ SOURCES += \
     src/comm/ros_bridge/include/TopicPublisher.cpp \
     src/comm/ros_bridge/include/TopicSubscriber.cpp \
     src/comm/ros_bridge/src/CasePacker.cpp \
-    src/Wima/Snake/snaketile.cpp \
     src/api/QGCCorePlugin.cc \
     src/api/QGCOptions.cc \
     src/api/QGCSettings.cc \

src/Snake/flight_plan.cpp

+#include "flight_plan.h"
+
+#include "clipper/clipper.hpp"
+#define CLIPPER_SCALE 10000
+
+#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"
+
+
+
+
+struct FlightPlan::RoutingDataModel{
+    Matrix<int64_t>                 distanceMatrix;
+    long                            numVehicles;
+    RoutingIndexManager::NodeIndex  depot;
+};
+
+FlightPlan::FlightPlan()
+{
+
+}
+
+bool FlightPlan::generate(double lineDistance, double minTransectLength)
+{
+    _waypointsENU.clear();
+    _waypoints.clear();
+    _arrivalPathIdx.clear();
+    _returnPathIdx.clear();
+
+#ifndef NDEBUG
+    _PathVertices.clear();
+#endif
+#ifdef SHOW_TIME
+    auto start = std::chrono::high_resolution_clock::now();
+#endif
+    if (!_generateTransects(lineDistance, minTransectLength))
+        return false;
+#ifdef SHOW_TIME
+    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
+    cout << endl;
+   cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl;
+#endif
+
+    //=======================================
+    // Route Transects using Google or-tools.
+    //=======================================
+    // Offset joined area.
+    const BoostPolygon &jArea = _scenario.getJoineAreaENU();
+    BoostPolygon jAreaOffset;
+    offsetPolygon(jArea, jAreaOffset, detail::offsetConstant);
+
+    // Create vertex list;
+    BoostLineString vertices;
+    size_t n_t = _transects.size()*2;
+    size_t n0  = n_t+1;
+    vertices.reserve(n0);
+    for (auto lstring : _transects){
+        for (auto vertex : lstring){
+            vertices.push_back(vertex);
+        }
+    }
+    vertices.push_back(_scenario.getHomePositonENU());
+
+    for (long i=0; i<long(jArea.outer().size())-1; ++i) {
+        vertices.push_back(jArea.outer()[i]);
+    }
+    for (auto ring : jArea.inners()) {
+        for (auto vertex : ring)
+            vertices.push_back(vertex);
+    }
+    size_t n1 = vertices.size();
+    // Generate routing model.
+    RoutingDataModel dataModel;
+    Matrix<double> connectionGraph(n1, n1);
+
+#ifdef SHOW_TIME
+    start = std::chrono::high_resolution_clock::now();
+#endif
+    _generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
+#ifdef SHOW_TIME
+    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
+    cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl;
+#endif
+
+    // Create Routing Index Manager.
+    RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles,
+                                dataModel.depot);
+    // Create Routing Model.
+    RoutingModel routing(manager);
+
+    // Create and register a transit callback.
+    const int transit_callback_index = routing.RegisterTransitCallback(
+        [&dataModel, &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 dataModel.distanceMatrix.get(from_node, to_node);
+        });
+
+    // Define cost of each arc.
+    routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);
+
+
+    // Define Constraints.
+    size_t n = _transects.size()*2;
+    Solver *solver = routing.solver();
+    for (size_t i=0; i<n; i=i+2){
+//        auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
+//        auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
+//        auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
+//        auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
+//        auto c = solver->MakeNonEquality(cond0, cond1);
+//        solver->AddConstraint(c);
+
+        // alternative
+        auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
+        auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
+        auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
+        auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
+        vector<IntVar*> conds{cond0, cond1};
+        auto c = solver->MakeAllDifferent(conds);
+        solver->MakeRejectFilter();
+        solver->AddConstraint(c);
+    }
+
+
+    // Setting first solution heuristic.
+    RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
+    searchParameters.set_first_solution_strategy(
+        FirstSolutionStrategy::PATH_CHEAPEST_ARC);
+    google::protobuf::Duration *tMax = new google::protobuf::Duration(); // seconds
+    tMax->set_seconds(10);
+    searchParameters.set_allocated_time_limit(tMax);
+
+    // Solve the problem.
+#ifdef SHOW_TIME
+    start = std::chrono::high_resolution_clock::now();
+#endif
+    const Assignment* solution = routing.SolveWithParameters(searchParameters);
+#ifdef SHOW_TIME
+    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
+    cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl;
+#endif
+
+    if (!solution || solution->Size() <= 1){
+        errorString = "Not able to solve the routing problem.";
+        return false;
+    }
+
+    // Extract waypoints from solution.
+    long index = routing.Start(0);
+    std::vector<size_t> route;
+    route.push_back(manager.IndexToNode(index).value());
+    while (!routing.IsEnd(index)){
+        index = solution->Value(routing.NextVar(index));
+        route.push_back(manager.IndexToNode(index).value());
+    }
+
+    // Connect transects
+#ifndef NDEBUG
+    _PathVertices = vertices;
+#endif
+
+    {
+    _waypointsENU.push_back(vertices[route[0]]);
+    vector<size_t> pathIdx;
+    long arrivalPathLength = 0;
+    for (long i=0; i<long(route.size())-1; ++i){
+        size_t idx0 = route[i];
+        size_t idx1 = route[i+1];
+        pathIdx.clear();
+        shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
+        if ( i==0 )
+            arrivalPathLength = pathIdx.size();
+        for (size_t j=1; j<pathIdx.size(); ++j)
+            _waypointsENU.push_back(vertices[pathIdx[j]]);
+    }
+
+
+    long returnPathLength = pathIdx.size();
+    for (long i=returnPathLength; i > 0; --i)
+        _returnPathIdx.push_back(_waypointsENU.size()-i);
+
+    for (long i=0; i < arrivalPathLength; ++i)
+        _arrivalPathIdx.push_back(i);
+    }
+
+    // Back transform waypoints.
+    GeoPoint3D origin{_scenario.getOrigin()};
+    for (auto vertex : _waypointsENU) {
+        GeoPoint3D geoVertex;
+        fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
+        _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
+    }
+
+    return true;
+}
+
+bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength)
+{
+    _transects.clear();
+    if (_scenario.getTilesENU().size() != _progress.size()){
+        ostringstream strstream;
+        strstream << "Number of tiles ("
+                  << _scenario.getTilesENU().size()
+                  << ") is not equal to progress array length ("
+                  << _progress.size()
+                  << ")";
+        errorString = strstream.str();
+        return false;
+    }
+
+    // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
+    size_t num_tiles = _progress.size();
+    vector<BoostPolygon> processedTiles;
+    {
+        const auto &tiles = _scenario.getTilesENU();
+        for (size_t i=0; i<num_tiles; ++i) {
+            if (_progress[i] == 100){
+                processedTiles.push_back(tiles[i]);
+            }
+        }
+
+        if (processedTiles.size() == num_tiles)
+            return true;
+    }
+
+    // Convert measurement area and tiles to clipper path.
+    ClipperLib::Path mAreaClipper;
+    for ( auto vertex : _scenario.getMeasurementAreaENU().outer() ){
+        mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
+                                                    static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
+    }
+    vector<ClipperLib::Path> processedTilesClipper;
+    for (auto t : processedTiles){
+        ClipperLib::Path path;
+        for (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);
+    }
+
+    const min_bbox_rt &bbox     = _scenario.getMeasurementAreaBBoxENU();
+    double alpha                = bbox.angle;
+    double x0                   = bbox.corners.outer()[0].get<0>();
+    double y0                   = bbox.corners.outer()[0].get<1>();
+    double bboxWidth            = bbox.width;
+    double bboxHeight           = bbox.height;
+    double delta                = detail::offsetConstant;
+
+    size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects
+    vector<double> yCoords;
+    yCoords.reserve(num_t);
+    double y = -delta;
+    for (size_t i=0; i < num_t; ++i) {
+        yCoords.push_back(y);
+        y += lineDistance;
+    }
+
+
+    // Generate transects and convert them to clipper path.
+    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-alpha*180/M_PI);
+    trans::translate_transformer<double, 2, 2> translate_back(x0, y0);
+    vector<ClipperLib::Path> transectsClipper;
+    transectsClipper.reserve(num_t);
+    for (size_t i=0; i < num_t; ++i) {
+        // calculate transect
+        BoostPoint v1{-delta, yCoords[i]};
+        BoostPoint v2{bboxWidth+delta, yCoords[i]};
+        BoostLineString transect;
+        transect.push_back(v1);
+        transect.push_back(v2);
+
+        // transform back
+        BoostLineString temp_transect;
+        bg::transform(transect, temp_transect, rotate_back);
+        transect.clear();
+        bg::transform(temp_transect, transect, translate_back);
+
+
+        ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(transect[0].get<0>()*CLIPPER_SCALE),
+                                static_cast<ClipperLib::cInt>(transect[0].get<1>()*CLIPPER_SCALE)};
+        ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(transect[1].get<0>()*CLIPPER_SCALE),
+                                static_cast<ClipperLib::cInt>(transect[1].get<1>()*CLIPPER_SCALE)};
+        ClipperLib::Path path{c1, c2};
+        transectsClipper.push_back(path);
+    }
+
+    // Perform clipping.
+    // Clip transects to measurement area.
+    ClipperLib::Clipper clipper;
+    clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
+    clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
+    ClipperLib::PolyTree clippedTransecsPolyTree1;
+    clipper.Execute(ClipperLib::ctIntersection, clippedTransecsPolyTree1, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
+
+    // Subtract holes (tiles with measurement_progress == 100) from transects.
+    clipper.Clear();
+    for (auto child : clippedTransecsPolyTree1.Childs)
+        clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
+    clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
+    ClipperLib::PolyTree clippedTransecsPolyTree2;
+    clipper.Execute(ClipperLib::ctDifference, clippedTransecsPolyTree2, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
+
+    // Extract transects from  PolyTree and convert them to BoostLineString
+    for (auto child : clippedTransecsPolyTree2.Childs){
+        ClipperLib::Path clipperTransect = child->Contour;
+        BoostPoint v1{static_cast<double>(clipperTransect[0].X)/CLIPPER_SCALE,
+                      static_cast<double>(clipperTransect[0].Y)/CLIPPER_SCALE};
+        BoostPoint v2{static_cast<double>(clipperTransect[1].X)/CLIPPER_SCALE,
+                      static_cast<double>(clipperTransect[1].Y)/CLIPPER_SCALE};
+
+        BoostLineString transect{v1, v2};
+        if (bg::length(transect) >= minTransectLength)
+            _transects.push_back(transect);
+
+    }
+
+    if (_transects.size() < 1)
+        return false;
+
+    return true;
+}
+
+void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
+                                       const BoostPolygon &polygonOffset,
+                                       size_t n0,
+                                       FlightPlan::RoutingDataModel &dataModel,
+                                       Matrix<double> &graph)
+{
+#ifdef SHOW_TIME
+    auto start = std::chrono::high_resolution_clock::now();
+#endif
+    graphFromPolygon(polygonOffset, vertices, graph);
+#ifdef SHOW_TIME
+    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
+    cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
+#endif
+//    cout << endl;
+//    for (size_t i=0; i<graph.size(); ++i){
+//        vector<double> &row = graph[i];
+//        for (size_t j=0; j<row.size();++j){
+//            cout << "(" << i << "," << j << "):" << row[j] << " ";
+//        }
+//        cout << endl;
+//    }
+//    cout << endl;
+    Matrix<double> distanceMatrix(graph);
+#ifdef SHOW_TIME
+    start = std::chrono::high_resolution_clock::now();
+#endif
+    toDistanceMatrix(distanceMatrix);
+#ifdef SHOW_TIME
+    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
+    cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;
+#endif
+
+    dataModel.distanceMatrix.setDimension(n0, n0);
+    for (size_t i=0; i<n0; ++i){
+        dataModel.distanceMatrix.set(i, i, 0);
+        for (size_t j=i+1; j<n0; ++j){
+            dataModel.distanceMatrix.set(i, j, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
+            dataModel.distanceMatrix.set(j, i, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
+        }
+    }
+
+    dataModel.numVehicles   = 1;
+    dataModel.depot         = n0-1;
+}

src/Snake/flight_plan.h

+#pragma once
+
+namespace flight_plan {
+
+
+
+//========================================================================================
+// FlightPlan
+//========================================================================================
+
+class FlightPlan{
+public:
+    FlightPlan();
+
+    void setScenario(Scenario &scenario)          {_scenario = scenario;}
+    void setProgress(vector<int> &progress)       {_progress = progress;}
+
+    const Scenario          &getScenario(void)          const {return _scenario;}
+    const BoostLineString   &getWaypointsENU(void)      const {return _waypointsENU;}
+    const GeoPoint2DList    &getWaypoints(void)         const {return _waypoints;}
+    const vector<uint64_t>  &getArrivalPathIdx(void)    const {return _arrivalPathIdx;}
+    const vector<uint64_t>  &getReturnPathIdx(void)     const {return _returnPathIdx;}
+#ifndef NDEBUG
+    const BoostLineString   &getPathVertices(void)      const {return _PathVertices;}
+#endif
+
+    bool                            generate(double lineDistance, double minTransectLength);
+    const vector<BoostLineString>  &getTransects() const {return _transects;}
+
+    string errorString;
+
+private:
+    // Search Filter to speed up routing.SolveWithParameters(...);
+    // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
+    class SearchFilter;
+
+    struct RoutingDataModel;
+
+    bool _generateTransects(double lineDistance, double minTransectLength);
+    void _generateRoutingModel(const BoostLineString &vertices,
+                               const BoostPolygon &polygonOffset,
+                               size_t n0,
+                               RoutingDataModel &dataModel,
+                               Matrix<double> &graph);
+
+    Scenario                                _scenario;
+    BoostLineString                         _waypointsENU;
+    GeoPoint2DList                          _waypoints;
+    vector<BoostLineString>                 _transects;
+    vector<int>                             _progress;
+    vector<uint64_t>                        _arrivalPathIdx;
+    vector<uint64_t>                        _returnPathIdx;
+
+#ifndef NDEBUG
+    BoostLineString                         _PathVertices;
+#endif
+};
+
+}

src/Snake/snake.cpp

 #include <iostream>
 
 #include "snake.h"
-#include "clipper/clipper.hpp"
-#define CLIPPER_SCALE 10000
 
-#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"
+#include <mapbox/polylabel.hpp>
+#include <mapbox/geometry.hpp>
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
 
 using namespace operations_research;
 
@@ -15,165 +15,453 @@ using namespace operations_research;
 //#define SHOW_TIME
 #endif
 
-using namespace snake_geometry;
-using namespace std;
-
 namespace bg = boost::geometry;
 namespace trans = bg::strategy::transform;
 
 namespace snake {
+//=========================================================================
+// Geometry stuff.
+//=========================================================================
+
+BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
 
-Scenario::Scenario() :
-    _mAreaBoundingBox(min_bbox_rt{0, 0, 0, BoostPolygon{}})
+void polygonCenter(const BoostPolygon &polygon, BoostPoint &center)
 {
+ using namespace mapbox;
+if (polygon.outer().empty())
+    return;
+geometry::polygon<double> p;
+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>());
+    lr1.push_back(vertex);
+}
+p.push_back(lr1);
+geometry::point<double> c = polylabel(p);
 
+center.set<0>(c.x);
+center.set<1>(c.y);
 }
 
-bool Scenario::addArea(Area &area)
-    {
-        if (area.geoPolygon.size() < 3){
-            errorString = "Area has less than three vertices.";
-            return false;
-        }
-        if (area.type == MeasurementArea)
-            return Scenario::_setMeasurementArea(area);
-        else if (area.type == ServiceArea)
-            return Scenario::_setServiceArea(area);
-        else if (area.type == Corridor)
-            return Scenario::_setCorridor(area);
-        return false;
+void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox)
+{
+ /*
+ Find the minimum-area bounding box of a set of 2D points
+
+ The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates.
+ The first and last points points must be the same, making a closed polygon.
+ This program finds the rotation angles of each edge of the convex polygon,
+ then tests the area of a bounding box aligned with the unique angles in
+ 90 degrees of the 1st Quadrant.
+ Returns the
+
+ Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version)
+ Results verified using Matlab
+
+ Copyright (c) 2013, David Butterworth, University of Queensland
+ All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+
+     * Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+     * Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+     * Neither the name of the Willow Garage, Inc. nor the names of its
+       contributors may be used to endorse or promote products derived from
+       this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGE.
+ */
+
+ if (polygon.outer().empty())
+     return;
+ BoostPolygon convex_hull;
+ bg::convex_hull(polygon, convex_hull);
+
+ //cout << "Convex hull: " << bg::wkt<BoostPolygon2D>(convex_hull) << endl;
+
+ //# Compute edges (x2-x1,y2-y1)
+ std::vector<BoostPoint> edges;
+ auto convex_hull_outer = convex_hull.outer();
+ for (long i=0; i < long(convex_hull_outer.size())-1; ++i) {
+     BoostPoint p1      = convex_hull_outer.at(i);
+     BoostPoint p2      = convex_hull_outer.at(i+1);
+     double edge_x   = p2.get<0>() - p1.get<0>();
+     double edge_y   = p2.get<1>() - p1.get<1>();
+     edges.push_back(BoostPoint{edge_x, edge_y});
+ }
+
+//    cout << "Edges: ";
+//    for (auto e : edges)
+//        cout << e.get<0>() << " " << e.get<1>() << ",";
+//    cout << endl;
+
+ // Calculate unique edge angles  atan2(y/x)
+ double angle_scale = 1e3;
+ std::set<long> angles_long;
+ for (auto vertex : edges) {
+     double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2);
+     angle = angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers
+     angles_long.insert(long(round(angle*angle_scale)));
+ }
+ std::vector<double> edge_angles;
+ for (auto a : angles_long)
+     edge_angles.push_back(double(a)/angle_scale);
+
+
+//    cout << "Unique angles: ";
+//    for (auto e : edge_angles)
+//        cout << e*180/M_PI << ",";
+//    cout << endl;
+
+ double min_area = std::numeric_limits<double>::infinity();
+ // Test each angle to find bounding box with smallest area
+ // print "Testing", len(edge_angles), "possible rotations for bounding box... \n"
+ for (double angle : edge_angles){
+
+     trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle*180/M_PI);
+     BoostPolygon hull_rotated;
+     bg::transform(convex_hull, hull_rotated, rotate);
+     //cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated) << endl;
+
+     bg::model::box<BoostPoint> box;
+     bg::envelope(hull_rotated, box);
+//        cout << "Bounding box: " << bg::wkt<bg::model::box<BoostPoint2D>>(box) << endl;
+
+     //# print "Rotated hull points are \n", rot_points
+     BoostPoint min_corner = box.min_corner();
+     BoostPoint max_corner = box.max_corner();
+     double min_x = min_corner.get<0>();
+     double max_x = max_corner.get<0>();
+     double min_y = min_corner.get<1>();
+     double max_y = max_corner.get<1>();
+//        cout << "min_x: " << min_x << endl;
+//        cout << "max_x: " << max_x << endl;
+//        cout << "min_y: " << min_y << endl;
+//        cout << "max_y: " << max_y << endl;
+
+     // Calculate height/width/area of this bounding rectangle
+     double width    = max_x - min_x;
+     double height   = max_y - min_y;
+     double area     = width * height;
+//        cout << "Width: " << width << endl;
+//        cout << "Height: " << height << endl;
+//        cout << "area: " << area << endl;
+//        cout << "angle: " << angle*180/M_PI << endl;
+
+     // Store the smallest rect found first (a simple convex hull might have 2 answers with same area)
+     if (area < min_area){
+         min_area = area;
+         minBBox.angle  = angle;
+         minBBox.width  = width;
+         minBBox.height = height;
+
+         minBBox.corners.clear();
+         minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
+         minBBox.corners.outer().push_back(BoostPoint{min_x, max_y});
+         minBBox.corners.outer().push_back(BoostPoint{max_x, max_y});
+         minBBox.corners.outer().push_back(BoostPoint{max_x, min_y});
+         minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
+     }
+     //cout << endl << endl;
+ }
+
+
+ // Transform corners of minimal bounding box.
+ trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-minBBox.angle*180/M_PI);
+ BoostPolygon rotated_polygon;
+ bg::transform(minBBox.corners, rotated_polygon, rotate);
+ minBBox.corners = rotated_polygon;
 }
 
-bool Scenario::update(double tileWidth, double tileHeight, double minTileArea)
+void toBoost(const Point2D &point, BoostPoint &boost_point)
 {
-    if (!_areas2enu())
-        return false;
-    if (!_calculateBoundingBox())
-        return false;
-    if (!_calculateTiles(tileWidth, tileHeight, minTileArea))
-        return false;
-    if (!_calculateJoinedArea())
-        return false;
+ boost_point.set<0>(point[0]);
+ boost_point.set<1>(point[1]);
+}
 
-    return true;
+void fromBoost(const BoostPoint &boost_point, Point2D &point)
+{
+ point[0] = boost_point.get<0>();
+ point[1] = boost_point.get<1>();
 }
 
-bool Scenario::_areas2enu()
+void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon)
 {
-    if (_measurementArea.geoPolygon.size() > 0){
-        _measurementAreaENU.clear();
-        for(auto vertex : _measurementArea.geoPolygon) {
-                Point3D ENUVertex;
-                toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _measurementArea.altitude}, ENUVertex);
-                _measurementAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
-        }
-        bg::correct(_measurementAreaENU);
-
-        _serviceAreaENU.clear();
-        if (_serviceArea.geoPolygon.size() > 0){
-            for(auto vertex : _serviceArea.geoPolygon) {
-                    Point3D ENUVertex;
-                    toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _serviceArea.altitude}, ENUVertex);
-                    _serviceAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
-            }
-        } else{
-            errorString = "Service area has no vertices.";
-            return false;
-        }
-        bg::correct(_serviceAreaENU);
-        polygonCenter(_serviceAreaENU, _homePositionENU);
-        fromENU(_geoOrigin, Point3D{_homePositionENU.get<0>(), _homePositionENU.get<1>(), 0}, _homePosition);
-
-        _corridorENU.clear();
-        if (_corridor.geoPolygon.size() > 0){
-            for(auto vertex : _corridor.geoPolygon) {
-                    Point3D ENUVertex;
-                    toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _corridor.altitude}, ENUVertex);
-                    _corridorENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
-            }
-        }
-        bg::correct(_corridorENU);
+ for (auto vertex : point_list) {
+     BoostPoint boost_vertex;
+     toBoost(vertex, boost_vertex);
+     boost_polygon.outer().push_back(boost_vertex);
+ }
+ bg::correct(boost_polygon);
+}
 
-        return true;
-    }
+void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list)
+{
+ for (auto boost_vertex : boost_polygon.outer()) {
+     Point2D vertex;
+     fromBoost(boost_vertex, vertex);
+     point_list.push_back(vertex);
+ }
+}
 
-    errorString = "Measurement area has no vertices.";
-    return false;
+void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree)
+{
+ trans::rotate_transformer<bg::degree, double, 2, 2> rotate(degree);
+ BoostPolygon boost_polygon;
+ toBoost(point_list, boost_polygon);
+ BoostPolygon rotated_polygon;
+ bg::transform(boost_polygon, rotated_polygon, rotate);
+ fromBoost(rotated_polygon, rotated_point_list);
 }
 
-bool Scenario::_setMeasurementArea(Area &area)
+void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad)
 {
-    if (area.geoPolygon.size() <= 0)
-        return false;
-    GeoPoint2D origin2D = area.geoPolygon[0];
-    _geoOrigin = GeoPoint3D{origin2D[0], origin2D[1], 0};
-    _measurementArea = area;
-    _measurementAreaENU.clear();
-    _serviceAreaENU.clear();
-    _corridorENU.clear();
-    return true;
+ rotateDeg(point_list, rotated_point_list, rad*180/M_PI);
+}
 
+bool isClockwise(const Point2DList &point_list)
+{
+ double orientaion = 0;
+ double len = point_list.size();
+ for (long i=0; i < len-1; ++i){
+     Point2D v1 = point_list[i];
+     Point2D v2 = point_list[i+1];
+     orientaion += (v2[0]-v1[0])*(v2[1]+v1[1]);
+ }
+ Point2D v1 = point_list[len-1];
+ Point2D v2 = point_list[0];
+ orientaion += (v2[0]-v1[0])*(v2[1]+v1[1]);
+
+ return orientaion > 0 ? true : false;
 }
 
-bool Scenario::_setServiceArea(Area &area)
+void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset)
 {
-    if (area.geoPolygon.size() <= 0)
-        return false;
-    _serviceArea = area;
-    _serviceAreaENU.clear();
-    return true;
+ bg::strategy::buffer::distance_symmetric<double> distance_strategy(offset);
+ bg::strategy::buffer::join_miter join_strategy(3);
+ bg::strategy::buffer::end_flat end_strategy;
+ bg::strategy::buffer::point_square point_strategy;
+ bg::strategy::buffer::side_straight side_strategy;
+
+
+ bg::model::multi_polygon<BoostPolygon> result;
+
+ bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy);
+
+ if (result.size() > 0)
+     polygonOffset = result[0];
+
 }
 
-bool Scenario::_setCorridor(Area &area)
+
+void graphFromPolygon(const BoostPolygon &polygon, const BoostLineString &vertices, Matrix<double> &graph)
 {
-    if (area.geoPolygon.size() <= 0)
-        return false;
-    _corridor = area;
-    _corridorENU.clear();
-    return true;
+ size_t n = graph.getN();
+
+ for (size_t i=0; i < n; ++i) {
+     BoostPoint v1 = vertices[i];
+     for (size_t j=i+1; j < n; ++j){
+         BoostPoint v2 = vertices[j];
+         BoostLineString path{v1, v2};
+
+         double distance = 0;
+         if (!bg::within(path, polygon))
+             distance = std::numeric_limits<double>::infinity();
+         else
+             distance = bg::length(path);
+
+         graph.set(i, j, distance);
+         graph.set(j, i, distance);
+     }
+ }
+
 }
 
-bool Scenario::_calculateBoundingBox()
+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)
 {
-    minimalBoundingBox(_measurementAreaENU, _mAreaBoundingBox);
-    return true;
+ if (    startIndex >= numElements
+      || endIndex >= numElements
+      || endIndex == startIndex) {
+     return false;
+ }
+ // Node struct
+ // predecessorIndex is the index of the predecessor node (nodeList[predecessorIndex])
+ // distance is the distance between the node and the start node
+ // node number is stored by the position in nodeList
+ struct Node{
+     int predecessorIndex = -1;
+     double distance = std::numeric_limits<double>::infinity();
+ };
+
+ // The list with all Nodes (elements)
+ std::vector<Node> nodeList(numElements);
+ // This list will be initalized with indices referring to the elements of nodeList.
+ // Elements will be successively remove during the execution of the Dijkstra Algorithm.
+ std::vector<size_t> workingSet(numElements);
+
+ //append elements to node list
+ for (size_t i = 0; i < numElements; ++i) workingSet[i] = i;
+
+
+ nodeList[startIndex].distance = 0;
+
+ // Dijkstra Algorithm
+ // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
+ while (workingSet.size() > 0) {
+     // serach Node with minimal distance
+     double minDist = std::numeric_limits<double>::infinity();
+     int minDistIndex_WS = -1; // WS = workinSet
+     for (size_t i = 0; i < workingSet.size(); ++i) {
+         const int nodeIndex = workingSet.at(i);
+         const double dist = nodeList.at(nodeIndex).distance;
+         if (dist < minDist) {
+             minDist = dist;
+             minDistIndex_WS = i;
+         }
+     }
+     if (minDistIndex_WS == -1)
+             return false;
+
+     size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList
+     workingSet.erase(workingSet.begin()+minDistIndex_WS);
+     if (indexU_NL == endIndex) // shortest path found
+         break;
+
+     const double distanceU = nodeList.at(indexU_NL).distance;
+     //update distance
+     for (size_t i = 0; i < workingSet.size(); ++i) {
+         int indexV_NL = workingSet[i]; // NL = nodeList
+         Node* v = &nodeList[indexV_NL];
+         double dist = distanceDij(indexU_NL, indexV_NL);
+         // is ther an alternative path which is shorter?
+         double alternative = distanceU + dist;
+         if (alternative < v->distance)  {
+             v->distance         = alternative;
+             v->predecessorIndex = indexU_NL;
+         }
+     }
+
+ }
+ // end Djikstra Algorithm
+
+
+ // reverse assemble path
+ int e = endIndex;
+ while (1) {
+     if (e == -1) {
+         if (elementPath[0] == startIndex)// check if starting point was reached
+             break;
+         return false;
+     }
+     elementPath.insert(elementPath.begin(), e);
+
+     //Update Node
+     e = nodeList[e].predecessorIndex;
+
+ }
+ return true;
 }
 
+void toDistanceMatrix(Matrix<double> &graph)
+{
+ size_t n = graph.getN();
+
+ auto distance = [graph](size_t i, size_t j){
+     return graph.get(i,j);
+ };
+
+
+ std::vector<size_t> path;
+ for (size_t i=0; i < n; ++i) {
+     for (size_t j=i+1; j < n; ++j){
+         double d = graph.get(i,j);
+         if (!std::isinf(d))
+             continue;
+         path.clear();
+         bool ret = dijkstraAlgorithm(n, i, j, path, distance);
+         assert(ret);
+         (void)ret;
+//            cout << "(" << i << "," << j << ") d: " << d << endl;
+//            cout << "Path size: " << path.size() << endl;
+//            for (auto idx : path)
+//                cout << idx << " ";
+//            cout << endl;
+
+         d = 0;
+         for (long k=0; k < long(path.size())-1; ++k) {
+             size_t idx0 = path[k];
+             size_t idx1 = path[k+1];
+             double d0 = graph.get(idx0, idx1);
+             assert(std::isinf(d0) == false);
+             d += d0;
+         }
+
+         graph.set(i, j, d);
+         graph.set(j, i, d);
+     }
+ }
+}
 
-/**
- * Devides the (measurement area) bounding  box into tiles and clips it to the measurement area.
- *
- * Devides the (measurement area) bounding  box into tiles of width \p tileWidth and height \p tileHeight.
- * Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea.
- * The function assumes that \a _measurementAreaENU and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref
- * Scenario::_calculateBoundingBox().
- *
- * @param tileWidth The width (>0) of a tile.
- * @param tileHeight The heigth (>0) of a tile.
- * @param minTileArea The minimal area (>0) of a tile.
- *
- * @return Returns true if successful.
- */
-bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTileArea)
+void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex, size_t endIndex, std::vector<size_t> &pathIdx)
 {
-    _tilesENU.clear();
-    _tileCenterPointsENU.clear();
-    _tiles.clear();
-    _tileCenterPoints.clear();
 
-    if (tileWidth <= 0 || tileHeight <= 0 || minTileArea <= 0) {
+ if (!std::isinf(graph.get(startIndex, endIndex))){
+     pathIdx.push_back(startIndex);
+     pathIdx.push_back(endIndex);
+ } else {
+     auto distance = [graph](size_t i, size_t j){
+         return graph.get(i, j);
+     };
+     bool ret = dijkstraAlgorithm(graph.getN(), startIndex, endIndex, pathIdx, distance);
+     assert(ret);
+     (void)ret;
+ }
+
+}
+
+//=========================================================================
+// Tile calculation.
+//=========================================================================
+bool calculateTiles(const BoostPolygon &mArea,
+                    double tileWidth,
+                    double tileHeight,
+                    double minTileArea,
+                    std::vector<BoostPolygon> &tiles,
+                    std::string &errorString)
+{
+    using namespace snake_geometry;
+    if (tileWidth <= 0 || tileHeight <= 0 || minTileArea < 0) {
         errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive.";
         return false;
     }
 
-    double bbox_width = _mAreaBoundingBox.width;
-    double bbox_height = _mAreaBoundingBox.height;
-
-    BoostPoint origin = _mAreaBoundingBox.corners.outer()[0];
+    BoundingBox bbox;
+    minimalBoundingBox(mArea, bbox);
+    double bbox_width   = bbox.width;
+    double bbox_height  = bbox.height;
+    BoostPoint origin   = bbox.corners.outer()[0];
 
     //cout << "Origin: " << origin[0] << " " << origin[1] << endl;
     // Transform _measurementAreaENU polygon to bounding box coordinate system.
-    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(_mAreaBoundingBox.angle*180/M_PI);
+    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>());
     BoostPolygon translated_polygon;
     BoostPolygon rotated_polygon;
@@ -187,12 +475,11 @@ bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTi
     size_t j_max = ceil(bbox_height/tileHeight);
 
     if (i_max < 1 || j_max < 1) {
-        errorString = "tileWidth or tileHeight to small.";
+        errorString = "tileWidth or tileHeight to large.";
         return false;
     }
 
-
-    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-_mAreaBoundingBox.angle*180/M_PI);
+    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 < i_max; ++i){
         double x_min = tileWidth*i;
@@ -223,118 +510,62 @@ bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTi
                     boost::geometry::transform(rotated_tile, translated_tile, translate_back);
 
                     // Store tile and calculate center point.
-                    _tilesENU.push_back(translated_tile);
-                    BoostPoint tile_center;
-                    polygonCenter(translated_tile, tile_center);
-                    _tileCenterPointsENU.push_back(tile_center);
+                    tiles.push_back(translated_tile);
                 }
            }
         }
     }
 
-    if (_tilesENU.size() < 1){
+    if (tiles.size() < 1){
         errorString = "No tiles calculated. Is the minTileArea parameter large enough?";
         return false;
     }
 
-    for ( auto tile : _tilesENU){
-        GeoPoint3DList geoTile;
-        for ( int i=0; i < int(tile.outer().size())-1; ++i) {
-            BoostPoint vertex(tile.outer()[i]);
-            GeoPoint3D geoVertex;
-            fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
-            geoTile.push_back(geoVertex);
-        }
-        _tiles.push_back(geoTile);
-    }
-
-    for ( auto vertex : _tileCenterPointsENU){
-        GeoPoint3D geoVertex;
-        fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
-        _tileCenterPoints.push_back(geoVertex);
-    }
-
     return true;
 }
 
-bool Scenario::_calculateJoinedArea()
+bool joinAreas(const std::vector<BoostPolygon> &areas, BoostPolygon &joinedArea)
 {
-    _joinedAreaENU.clear();
-    // Measurement area and service area overlapping?
-    bool overlapingSerMeas = bg::intersects(_measurementAreaENU, _serviceAreaENU) ? true : false;
-    bool corridorValid = _corridorENU.outer().size() > 0 ? true : false;
-
-
-    // Check if corridor is connecting measurement area and service area.
-    bool corridor_is_connection = false;
-    if (corridorValid) {
-        // Corridor overlaping with measurement area?
-        if ( bg::intersects(_corridorENU, _measurementAreaENU) ) {
-            // Corridor overlaping with service area?
-            if ( bg::intersects(_corridorENU, _serviceAreaENU) ) {
-                corridor_is_connection = true;
-            }
-        }
-    }
+    if (areas.size() < 1)
+        return false;
+    std::deque<std::size_t> idxList;
+    for(size_t i = 1; i < areas.size(); ++i)
+        idxList.push_back(i);
+    BoostPolygon partialArea = areas[0];
 
-    // Are areas joinable?
     std::deque<BoostPolygon> sol;
-    BoostPolygon partialArea = _measurementAreaENU;
-    if (overlapingSerMeas){
-        if(corridor_is_connection){
-            bg::union_(partialArea, _corridorENU, sol);
+    while (idxList.size() > 0){
+        bool success = false;
+        for (auto it = idxList.begin(); it != idxList.end(); ++it){
+            bg::union_(partialArea, areas[*it], sol);
+            if (sol.size() > 0) {
+                partialArea = sol[0];
+                sol.clear;
+                idxList.erase(it);
+                success = true;
+                break;
+            }
         }
-    } else if (corridor_is_connection){
-        bg::union_(partialArea, _corridorENU, sol);
-    } else {
-        errorString = "Areas are not overlapping";
-        return false;
-    }
-
-    if (sol.size() > 0) {
-        partialArea = sol[0];
-        sol.clear();
-    }
-
-    // Join areas.
-    bg::union_(partialArea, _serviceAreaENU, sol);
-
-    if (sol.size() > 0) {
-        _joinedAreaENU = sol[0];
-    } else {
-        return false;
+        if ( !success )
+            return false;
     }
-
-    return true;
 }
 
-
-
-struct FlightPlan::RoutingDataModel{
-    Matrix<int64_t>                 distanceMatrix;
-    long                            numVehicles;
-    RoutingIndexManager::NodeIndex  depot;
-};
-
-FlightPlan::FlightPlan()
+bool waypoints(const BoostPolygon &mArea,
+               const BoostPolygon &jArea,
+               std::vector<BoostPolygon> &tiles,
+               std::vector<long> &progress,
+               BoostPoint &home,
+               double lineDistance,
+               double minTransectLength,
+               std::vector<BoostPoint>,
+               size_t arrivalPathLength,
+               size_t returnPathLength)
 {
-
-}
-
-bool FlightPlan::generate(double lineDistance, double minTransectLength)
-{
-    _waypointsENU.clear();    
-    _waypoints.clear();
-    _arrivalPathIdx.clear();
-    _returnPathIdx.clear();
-
-#ifndef NDEBUG
-    _PathVertices.clear();
-#endif
 #ifdef SHOW_TIME
     auto start = std::chrono::high_resolution_clock::now();
 #endif
-    if (!_generateTransects(lineDistance, minTransectLength))
+    if (!_generateTransects(mArea, lineDistance, minTransectLength, transects))
         return false;
 #ifdef SHOW_TIME
     auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
@@ -346,21 +577,20 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
     // Route Transects using Google or-tools.
     //=======================================
     // Offset joined area.
-    const BoostPolygon &jArea = _scenario.getJoineAreaENU();
     BoostPolygon jAreaOffset;
     offsetPolygon(jArea, jAreaOffset, detail::offsetConstant);
 
     // Create vertex list;
     BoostLineString vertices;
-    size_t n_t = _transects.size()*2;
+    size_t n_t = transects.size()*2;
     size_t n0  = n_t+1;
     vertices.reserve(n0);
-    for (auto lstring : _transects){
+    for (auto lstring : transects){
         for (auto vertex : lstring){
             vertices.push_back(vertex);
         }
     }
-    vertices.push_back(_scenario.getHomePositonENU());
+    vertices.push_back(home);
 
     for (long i=0; i<long(jArea.outer().size())-1; ++i) {
         vertices.push_back(jArea.outer()[i]);
@@ -384,7 +614,8 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
 #endif
 
     // Create Routing Index Manager.
-    RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles,
+    RoutingIndexManager manager(dataModel.distanceMatrix.getN(),
+                                dataModel.numVehicles,
                                 dataModel.depot);
     // Create Routing Model.
     RoutingModel routing(manager);
@@ -458,14 +689,9 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
     }
 
     // Connect transects
-#ifndef NDEBUG
-    _PathVertices = vertices;
-#endif
-
-    {
-    _waypointsENU.push_back(vertices[route[0]]);
+    waypoint.push_back(vertices[route[0]]);
     vector<size_t> pathIdx;
-    long arrivalPathLength = 0;
+    arrivalPathLength = 0;
     for (long i=0; i<long(route.size())-1; ++i){
         size_t idx0 = route[i];
         size_t idx1 = route[i+1];
@@ -474,33 +700,23 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
         if ( i==0 )
             arrivalPathLength = pathIdx.size();
         for (size_t j=1; j<pathIdx.size(); ++j)
-            _waypointsENU.push_back(vertices[pathIdx[j]]);
-    }
-
-
-    long returnPathLength = pathIdx.size();
-    for (long i=returnPathLength; i > 0; --i)
-        _returnPathIdx.push_back(_waypointsENU.size()-i);
-
-    for (long i=0; i < arrivalPathLength; ++i)
-        _arrivalPathIdx.push_back(i);
-    }
-
-    // Back transform waypoints.
-    GeoPoint3D origin{_scenario.getOrigin()};
-    for (auto vertex : _waypointsENU) {
-        GeoPoint3D geoVertex;
-        fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
-        _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
+            waypoints.push_back(vertices[pathIdx[j]]);
     }
 
+    returnPathLength = pathIdx.size();
     return true;
+
 }
 
-bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength)
+bool FlightPlan::_generateTransects(double lineDistance,
+                                    double minTransectLength,
+                                    const BoostPolygon mArea,
+                                    const std::vector<BoostPolygon> &tiles,
+                                    const std::vector<long> &progress,
+                                    vector<BoostLineString> &transects,
+                                    std::string &errorString)
 {
-    _transects.clear();
-    if (_scenario.getTilesENU().size() != _progress.size()){
+    if (tiles.size() != progress.size()){
         ostringstream strstream;
         strstream << "Number of tiles ("
                   << _scenario.getTilesENU().size()
@@ -512,12 +728,11 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
     }
 
     // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
-    size_t num_tiles = _progress.size();
+    size_t num_tiles = progress.size();
     vector<BoostPolygon> processedTiles;
     {
-        const auto &tiles = _scenario.getTilesENU();
-        for (size_t i=0; i<num_tiles; ++i) {
-            if (_progress[i] == 100){
+        for (size_t i = 0; i < num_tiles; ++i) {
+            if (progress[i] == 100){
                 processedTiles.push_back(tiles[i]);
             }
         }
@@ -528,9 +743,13 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
 
     // Convert measurement area and tiles to clipper path.
     ClipperLib::Path mAreaClipper;
-    for ( auto vertex : _scenario.getMeasurementAreaENU().outer() ){
-        mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
-                                                    static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
+    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)
+                    }
+                    );
     }
     vector<ClipperLib::Path> processedTilesClipper;
     for (auto t : processedTiles){
@@ -542,7 +761,8 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
         processedTilesClipper.push_back(path);
     }
 
-    const min_bbox_rt &bbox     = _scenario.getMeasurementAreaBBoxENU();
+    const BoundingBox bbox;
+    minimalBoundingBox(mArea, bbox);
     double alpha                = bbox.angle;
     double x0                   = bbox.corners.outer()[0].get<0>();
     double y0                   = bbox.corners.outer()[0].get<1>();
@@ -614,79 +834,14 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
 
         BoostLineString transect{v1, v2};
         if (bg::length(transect) >= minTransectLength)
-            _transects.push_back(transect);
+            transects.push_back(transect);
 
     }
 
-    if (_transects.size() < 1)
+    if (transects.size() < 1)
         return false;
 
     return true;
 }
 
-void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
-                                       const BoostPolygon &polygonOffset,
-                                       size_t n0,
-                                       FlightPlan::RoutingDataModel &dataModel,
-                                       Matrix<double> &graph)
-{
-#ifdef SHOW_TIME
-    auto start = std::chrono::high_resolution_clock::now();
-#endif
-    graphFromPolygon(polygonOffset, vertices, graph);
-#ifdef SHOW_TIME
-    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
-    cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
-#endif
-//    cout << endl;
-//    for (size_t i=0; i<graph.size(); ++i){
-//        vector<double> &row = graph[i];
-//        for (size_t j=0; j<row.size();++j){
-//            cout << "(" << i << "," << j << "):" << row[j] << " ";
-//        }
-//        cout << endl;
-//    }
-//    cout << endl;
-    Matrix<double> distanceMatrix(graph);
-#ifdef SHOW_TIME
-    start = std::chrono::high_resolution_clock::now();
-#endif
-    toDistanceMatrix(distanceMatrix);
-#ifdef SHOW_TIME
-    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
-    cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;
-#endif
-
-    dataModel.distanceMatrix.setDimension(n0, n0);
-    for (size_t i=0; i<n0; ++i){
-        dataModel.distanceMatrix.set(i, i, 0);
-        for (size_t j=i+1; j<n0; ++j){
-            dataModel.distanceMatrix.set(i, j, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
-            dataModel.distanceMatrix.set(j, i, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
-        }
-    }
-
-    dataModel.numVehicles   = 1;
-    dataModel.depot         = n0-1;
-}
-
-Area::Area() : Area(GeoPoint2DList(), 0, 1, AreaType::MeasurementArea)
-{
-
-}
-
-Area::Area(const GeoPoint2DList &gP, AreaType tp) : Area(gP, 0, 1, tp)
-{
-
-}
-
-Area::Area(const GeoPoint2DList &gP, double alt, size_t l, AreaType tp) :
-    geoPolygon(gP)
-  , altitude(alt)
-  , layers(l)
-  , type(tp)
-{
-
-}
-
 }

src/Snake/snake.h

@@ -4,142 +4,191 @@
 #include <string>
 #include <array>
 
+#include "snake_typedefs.h"
 #include "snake_geometry.h"
 
+#include <GeographicLib/Geocentric.hpp>
+#include <GeographicLib/LocalCartesian.hpp>
+
 using namespace std;
-using namespace snake_geometry;
 
 namespace snake {
 
-    enum AreaType {MeasurementArea, ServiceArea, Corridor};
+//=========================================================================
+// Geometry stuff.
+//=========================================================================
+template<class T>
+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);
+    }
+
+    double get(size_t i, size_t j) const
+    {
+        assert(_isInitialized);
+        assert(i < _m);
+        assert(j < _n);
+        return _matrix[i*_m+j];
+    }
+
+    size_t getM() const { return _m;}
+    size_t getN() const { return _n;}
 
-    class Area {
-    public:
-        Area();
-        Area(const GeoPoint2DList &gP, AreaType tp);
-        Area(const GeoPoint2DList &gP, double alt, size_t l, AreaType tp);
+    void set(size_t i, size_t j, const T &value)
+    {
+        assert(_isInitialized);
+        assert(i < _m);
+        assert(j < _n);
+        _matrix[i*_m+j] = value;
+    }
 
-        GeoPoint2DList      geoPolygon;
-        double              altitude;
-        size_t              layers;
-        AreaType            type;
-    };
+    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;
+    }
+
+    void setDimension(size_t m, size_t n)
+    {
+        setDimension(m, n, T{0});
+    }
+
+private:
+    size_t _elements;
+    size_t _m;
+    size_t _n;
+    bool   _isInitialized;
+    std::vector<T> _matrix;
+};
+
+typedef struct {
+    double width;
+    double height;
+    double angle;
+    BoostPolygon corners;
+}BoundingBox;
+
+template<class GeoPoint>
+void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out)
+{
+    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
+                                    GeographicLib::Constants::WGS84_f());
+    GeographicLib::LocalCartesian proj(origin.latitude(),
+                                       origin.longitude(),
+                                       origin.altitude(),
+                                       earth);
+
+    double x, y, z;
+    proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z);
+    out.set<0>(x);
+    out.set<0>(y);
+    (void)z;
+}
+template<class GeoPoint>
+void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out)
+{
+    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
+                                    GeographicLib::Constants::WGS84_f());
+    GeographicLib::LocalCartesian proj(origin.latitude(),
+                                       origin.longitude(),
+                                       origin.altitude(),
+                                       earth);
+
+    double lat, lon, alt;
+    proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt);
+    out.setLatitude(lat);
+    out.setLongitude(lon);
+    out.setAltitude(alt);
+}
+
+void polygonCenter(const BoostPolygon   &polygon,
+                   BoostPoint           &center);
+void minimalBoundingBox(const BoostPolygon  &polygon,
+                        BoundingBox         &minBBox);
+void offsetPolygon(const BoostPolygon   &polygon,
+                   BoostPolygon         &polygonOffset,
+                   double               offset);
+
+void graphFromPolygon(const BoostPolygon    &polygon,
+                      const BoostLineString &vertices,
+                      Matrix<double>        &graph);
+void 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);
+
+//=========================================================================
+// Tile calculation.
+//=========================================================================
+template<class GeoPoint, template <class, class...> class Container>
+void areaToEnu( const GeoPoint              &origin,
+                const Container<GeoPoint>   &in,
+                BoostPolygon                &out )
+{
+    for(auto vertex : in) {
+            BoostPoint p;
+            toENU(origin, vertex, p);
+            out.outer().push_back(p);
+    }
+    bg::correct(_measurementAreaENU);
+}
+
+template<class GeoPoint, template <class, class...> class Container>
+void areaFromEnu( const GeoPoint              &origin,
+                  BoostPolygon                &in,
+                  const Container<GeoPoint>   &out )
+{
+    for(auto vertex : in.outer()) {
+            GeoPoint p;
+            fromENU(origin, vertex, p);
+            out.push_back(p);
+    }
+}
 
-    //========================================================================================
-    // Scenario
-    //========================================================================================
-    class Scenario{
-    public:
-        Scenario();
-
-        bool addArea(Area &area);
-
-        const Area &getMeasurementArea() const {return _measurementArea;}
-        const Area &getServiceArea() const {return _serviceArea;}
-        const Area &getCorridor() const {return _corridor;}
-
-        const BoostPolygon &getMeasurementAreaENU() {return _measurementAreaENU;}
-        const BoostPolygon &getServiceAreaENU()     {return _serviceAreaENU;}
-        const BoostPolygon &getCorridorENU()        {return _corridorENU;}
-        const BoostPolygon &getJoineAreaENU()       {return _joinedAreaENU;}
-        const GeoPoint3D   &getOrigin()             {return _geoOrigin;}
-
-        const vector<BoostPolygon> &getTilesENU()               {return _tilesENU;}
-        const BoostLineString      &getTileCenterPointsENU()    {return _tileCenterPointsENU;}
-        const min_bbox_rt          &getMeasurementAreaBBoxENU() {return _mAreaBoundingBox;}
-        const BoostPoint           &getHomePositonENU()         {return _homePositionENU;}
-
-        const vector<GeoPoint3DList>    &getTiles()               {return _tiles;}
-        const vector<GeoPoint3D>        &getTileCenterPoints()    {return _tileCenterPoints;}
-        const GeoPoint3D                &getHomePositon()         {return _homePosition;}
-
-        bool update(double tileWidth, double tileHeight, double minTileArea);
-
-        string errorString;
-
-    private:
-        bool _areas2enu();
-        bool _setMeasurementArea(Area &area);
-        bool _setServiceArea(Area &area);
-        bool _setCorridor(Area &area);
-        bool _calculateBoundingBox();
-        bool _calculateTiles(double tileWidth, double tileHeight, double minTileArea);
-        bool _calculateJoinedArea();
-
-        Area _measurementArea;
-        Area _serviceArea;
-        Area _corridor;
-
-        BoostPolygon _measurementAreaENU;
-        BoostPolygon _serviceAreaENU;
-        BoostPolygon _corridorENU;
-        BoostPolygon _joinedAreaENU;
-
-        min_bbox_rt _mAreaBoundingBox;
-
-        vector<BoostPolygon>    _tilesENU;
-        BoostLineString         _tileCenterPointsENU;
-
-        vector<GeoPoint3DList>    _tiles;
-        vector<GeoPoint3D>        _tileCenterPoints;
-
-        GeoPoint3D _geoOrigin;
-        BoostPoint _homePositionENU;
-
-        GeoPoint3D _homePosition;
-    };
-
-    //========================================================================================
-    // FlightPlan
-    //========================================================================================
-
-    class FlightPlan{
-    public:
-        FlightPlan();
-
-        void setScenario(Scenario &scenario)          {_scenario = scenario;}
-        void setProgress(vector<int> &progress)       {_progress = progress;}
-
-        const Scenario          &getScenario(void)          const {return _scenario;}
-        const BoostLineString   &getWaypointsENU(void)      const {return _waypointsENU;}
-        const GeoPoint2DList    &getWaypoints(void)         const {return _waypoints;}
-        const vector<uint64_t>  &getArrivalPathIdx(void)    const {return _arrivalPathIdx;}
-        const vector<uint64_t>  &getReturnPathIdx(void)     const {return _returnPathIdx;}
-#ifndef NDEBUG
-        const BoostLineString   &getPathVertices(void)      const {return _PathVertices;}
-#endif
-
-        bool                            generate(double lineDistance, double minTransectLength);
-        const vector<BoostLineString>  &getTransects() const {return _transects;}
-
-        string errorString;
-
-    private:
-        // Search Filter to speed up routing.SolveWithParameters(...);
-        // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
-        class SearchFilter;
-
-        struct RoutingDataModel;
-
-        bool _generateTransects(double lineDistance, double minTransectLength);
-        void _generateRoutingModel(const BoostLineString &vertices,
-                                   const BoostPolygon &polygonOffset,
-                                   size_t n0,
-                                   RoutingDataModel &dataModel,
-                                   Matrix<double> &graph);
-
-        Scenario                                _scenario;
-        BoostLineString                         _waypointsENU;
-        GeoPoint2DList                          _waypoints;
-        vector<BoostLineString>                 _transects;
-        vector<int>                             _progress;
-        vector<uint64_t>                        _arrivalPathIdx;
-        vector<uint64_t>                        _returnPathIdx;
-
-#ifndef NDEBUG
-        BoostLineString                         _PathVertices;
-#endif
-    };
+bool calculateTiles(const BoostPolygon          &mArea,
+                    double                      tileWidth,
+                    double                      tileHeight,
+                    double                      minTileArea,
+                    std::vector<BoostPolygon>   &tiles,
+                    std::string                 &errorString);
+bool joinAreas(const std::vector<BoostPolygon>  &areas,
+               BoostPolygon                     &joinedArea);
+
+
+//=========================================================================
+// Waypoint calculation.
+//=========================================================================
+bool waypoints(const BoostPolygon &mArea, const BoostPolygon &jArea,
+               std::vector<BoostPolygon> &tiles,
+               BoostPoint &home,
+               double lineDistance,
+               double minTransectLength, std::vector<BoostPoint>, size_t arrivalPathLength, size_t returnPathLength);
 
     namespace detail {
         const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies.

src/Snake/snake_geometry.cpp

-#include "snake_geometry.h"
-#include <mapbox/polylabel.hpp>
-#include <mapbox/geometry.hpp>
-
-#include <boost/geometry.hpp>
-#include <boost/geometry/geometries/polygon.hpp>
-#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
-
-
-#include <GeographicLib/Geocentric.hpp>
-#include <GeographicLib/LocalCartesian.hpp>
-
-using namespace mapbox;
-using namespace snake_geometry;
-using namespace std;
-
-namespace bg = bg;
-namespace trans = bg::strategy::transform;
-
-BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
-
-
-namespace snake_geometry {
-
-void toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position, Point3D &ENUPosition)
-{
-    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
-    GeographicLib::LocalCartesian proj(WGS84Reference[0], WGS84Reference[1], WGS84Reference[2], earth);
-
-    proj.Forward(WGS84Position[0], WGS84Position[1], WGS84Position[2], ENUPosition[0], ENUPosition[1], ENUPosition[2]);
-}
-
-void fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition, GeoPoint3D &GeoPosition)
-{
-    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
-    GeographicLib::LocalCartesian proj(WGS84Reference[0], WGS84Reference[1], WGS84Reference[2], earth);
-
-    proj.Reverse(CartesianPosition[0], CartesianPosition[1], CartesianPosition[2], GeoPosition[0], GeoPosition[1], GeoPosition[2]);
-}
-
-void polygonCenter(const BoostPolygon &polygon, BoostPoint &center)
-{
-   if (polygon.outer().empty())
-       return;
-   geometry::polygon<double> p;
-   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>());
-       lr1.push_back(vertex);
-   }
-   p.push_back(lr1);
-   geometry::point<double> c = polylabel(p);
-
-   center.set<0>(c.x);
-   center.set<1>(c.y);
-}
-
-void minimalBoundingBox(const BoostPolygon &polygon, min_bbox_rt &minBBox)
-{
-    /*
-    Find the minimum-area bounding box of a set of 2D points
-
-    The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates.
-    The first and last points points must be the same, making a closed polygon.
-    This program finds the rotation angles of each edge of the convex polygon,
-    then tests the area of a bounding box aligned with the unique angles in
-    90 degrees of the 1st Quadrant.
-    Returns the
-
-    Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version)
-    Results verified using Matlab
-
-    Copyright (c) 2013, David Butterworth, University of Queensland
-    All rights reserved.
-
-    Redistribution and use in source and binary forms, with or without
-    modification, are permitted provided that the following conditions are met:
-
-        * Redistributions of source code must retain the above copyright
-          notice, this list of conditions and the following disclaimer.
-        * Redistributions in binary form must reproduce the above copyright
-          notice, this list of conditions and the following disclaimer in the
-          documentation and/or other materials provided with the distribution.
-        * Neither the name of the Willow Garage, Inc. nor the names of its
-          contributors may be used to endorse or promote products derived from
-          this software without specific prior written permission.
-
-    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-    LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-    CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-    SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-    INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-    CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-    ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-    POSSIBILITY OF SUCH DAMAGE.
-    */
-
-    if (polygon.outer().empty())
-        return;
-    BoostPolygon convex_hull;
-    bg::convex_hull(polygon, convex_hull);
-
-    //cout << "Convex hull: " << bg::wkt<BoostPolygon2D>(convex_hull) << endl;
-
-    //# Compute edges (x2-x1,y2-y1)
-    std::vector<BoostPoint> edges;
-    auto convex_hull_outer = convex_hull.outer();
-    for (long i=0; i < long(convex_hull_outer.size())-1; ++i) {
-        BoostPoint p1      = convex_hull_outer.at(i);
-        BoostPoint p2      = convex_hull_outer.at(i+1);
-        double edge_x   = p2.get<0>() - p1.get<0>();
-        double edge_y   = p2.get<1>() - p1.get<1>();
-        edges.push_back(BoostPoint{edge_x, edge_y});
-    }
-
-//    cout << "Edges: ";
-//    for (auto e : edges)
-//        cout << e.get<0>() << " " << e.get<1>() << ",";
-//    cout << endl;
-
-    // Calculate unique edge angles  atan2(y/x)
-    double angle_scale = 1e3;
-    std::set<long> angles_long;
-    for (auto vertex : edges) {
-        double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2);
-        angle = angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers
-        angles_long.insert(long(round(angle*angle_scale)));
-    }
-    std::vector<double> edge_angles;
-    for (auto a : angles_long)
-        edge_angles.push_back(double(a)/angle_scale);
-
-
-//    cout << "Unique angles: ";
-//    for (auto e : edge_angles)
-//        cout << e*180/M_PI << ",";
-//    cout << endl;
-
-    double min_area = std::numeric_limits<double>::infinity();
-    // Test each angle to find bounding box with smallest area
-    // print "Testing", len(edge_angles), "possible rotations for bounding box... \n"
-    for (double angle : edge_angles){
-
-        trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle*180/M_PI);
-        BoostPolygon hull_rotated;
-        bg::transform(convex_hull, hull_rotated, rotate);
-        //cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated) << endl;
-
-        bg::model::box<BoostPoint> box;
-        bg::envelope(hull_rotated, box);
-//        cout << "Bounding box: " << bg::wkt<bg::model::box<BoostPoint2D>>(box) << endl;
-
-        //# print "Rotated hull points are \n", rot_points
-        BoostPoint min_corner = box.min_corner();
-        BoostPoint max_corner = box.max_corner();
-        double min_x = min_corner.get<0>();
-        double max_x = max_corner.get<0>();
-        double min_y = min_corner.get<1>();
-        double max_y = max_corner.get<1>();
-//        cout << "min_x: " << min_x << endl;
-//        cout << "max_x: " << max_x << endl;
-//        cout << "min_y: " << min_y << endl;
-//        cout << "max_y: " << max_y << endl;
-
-        // Calculate height/width/area of this bounding rectangle
-        double width    = max_x - min_x;
-        double height   = max_y - min_y;
-        double area     = width * height;
-//        cout << "Width: " << width << endl;
-//        cout << "Height: " << height << endl;
-//        cout << "area: " << area << endl;
-//        cout << "angle: " << angle*180/M_PI << endl;
-
-        // Store the smallest rect found first (a simple convex hull might have 2 answers with same area)
-        if (area < min_area){
-            min_area = area;
-            minBBox.angle  = angle;
-            minBBox.width  = width;
-            minBBox.height = height;
-
-            minBBox.corners.clear();
-            minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
-            minBBox.corners.outer().push_back(BoostPoint{min_x, max_y});
-            minBBox.corners.outer().push_back(BoostPoint{max_x, max_y});
-            minBBox.corners.outer().push_back(BoostPoint{max_x, min_y});
-            minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
-        }
-        //cout << endl << endl;
-    }
-
-
-    // Transform corners of minimal bounding box.
-    trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-minBBox.angle*180/M_PI);
-    BoostPolygon rotated_polygon;
-    bg::transform(minBBox.corners, rotated_polygon, rotate);
-    minBBox.corners = rotated_polygon;
-}
-
-void toBoost(const Point2D &point, BoostPoint &boost_point)
-{
-    boost_point.set<0>(point[0]);
-    boost_point.set<1>(point[1]);
-}
-
-void fromBoost(const BoostPoint &boost_point, Point2D &point)
-{
-    point[0] = boost_point.get<0>();
-    point[1] = boost_point.get<1>();
-}
-
-void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon)
-{
-    for (auto vertex : point_list) {
-        BoostPoint boost_vertex;
-        toBoost(vertex, boost_vertex);
-        boost_polygon.outer().push_back(boost_vertex);
-    }
-    bg::correct(boost_polygon);
-}
-
-void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list)
-{
-    for (auto boost_vertex : boost_polygon.outer()) {
-        Point2D vertex;
-        fromBoost(boost_vertex, vertex);
-        point_list.push_back(vertex);
-    }
-}
-
-void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree)
-{
-    trans::rotate_transformer<bg::degree, double, 2, 2> rotate(degree);
-    BoostPolygon boost_polygon;
-    toBoost(point_list, boost_polygon);
-    BoostPolygon rotated_polygon;
-    bg::transform(boost_polygon, rotated_polygon, rotate);
-    fromBoost(rotated_polygon, rotated_point_list);
-}
-
-void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad)
-{
-    rotateDeg(point_list, rotated_point_list, rad*180/M_PI);
-}
-
-bool isClockwise(const Point2DList &point_list)
-{
-    double orientaion = 0;
-    double len = point_list.size();
-    for (long i=0; i < len-1; ++i){
-        Point2D v1 = point_list[i];
-        Point2D v2 = point_list[i+1];
-        orientaion += (v2[0]-v1[0])*(v2[1]+v1[1]);
-    }
-    Point2D v1 = point_list[len-1];
-    Point2D v2 = point_list[0];
-    orientaion += (v2[0]-v1[0])*(v2[1]+v1[1]);
-
-    return orientaion > 0 ? true : false;
-}
-
-void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset)
-{
-    bg::strategy::buffer::distance_symmetric<double> distance_strategy(offset);
-    bg::strategy::buffer::join_miter join_strategy(3);
-    bg::strategy::buffer::end_flat end_strategy;
-    bg::strategy::buffer::point_square point_strategy;
-    bg::strategy::buffer::side_straight side_strategy;
-
-
-    bg::model::multi_polygon<BoostPolygon> result;
-
-    bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy);
-
-    if (result.size() > 0)
-        polygonOffset = result[0];
-
-}
-
-
-void graphFromPolygon(const BoostPolygon &polygon, const BoostLineString &vertices, Matrix<double> &graph)
-{
-    size_t n = graph.getN();
-
-    for (size_t i=0; i < n; ++i) {
-        BoostPoint v1 = vertices[i];
-        for (size_t j=i+1; j < n; ++j){
-            BoostPoint v2 = vertices[j];
-            BoostLineString path{v1, v2};
-
-            double distance = 0;
-            if (!bg::within(path, polygon))
-                distance = std::numeric_limits<double>::infinity();
-            else
-                distance = bg::length(path);
-
-            graph.set(i, j, distance);
-            graph.set(j, i, distance);
-        }
-    }
-
-}
-
-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)
-{
-    if (    startIndex >= numElements
-         || endIndex >= numElements
-         || endIndex == startIndex) {
-        return false;
-    }
-    // Node struct
-    // predecessorIndex is the index of the predecessor node (nodeList[predecessorIndex])
-    // distance is the distance between the node and the start node
-    // node number is stored by the position in nodeList
-    struct Node{
-        int predecessorIndex = -1;
-        double distance = std::numeric_limits<double>::infinity();
-    };
-
-    // The list with all Nodes (elements)
-    std::vector<Node> nodeList(numElements);
-    // This list will be initalized with indices referring to the elements of nodeList.
-    // Elements will be successively remove during the execution of the Dijkstra Algorithm.
-    std::vector<size_t> workingSet(numElements);
-
-    //append elements to node list
-    for (size_t i = 0; i < numElements; ++i) workingSet[i] = i;
-
-
-    nodeList[startIndex].distance = 0;
-
-    // Dijkstra Algorithm
-    // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
-    while (workingSet.size() > 0) {
-        // serach Node with minimal distance
-        double minDist = std::numeric_limits<double>::infinity();
-        int minDistIndex_WS = -1; // WS = workinSet
-        for (size_t i = 0; i < workingSet.size(); ++i) {
-            const int nodeIndex = workingSet.at(i);
-            const double dist = nodeList.at(nodeIndex).distance;
-            if (dist < minDist) {
-                minDist = dist;
-                minDistIndex_WS = i;
-            }
-        }
-        if (minDistIndex_WS == -1)
-                return false;
-
-        size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList
-        workingSet.erase(workingSet.begin()+minDistIndex_WS);
-        if (indexU_NL == endIndex) // shortest path found
-            break;
-
-        const double distanceU = nodeList.at(indexU_NL).distance;
-        //update distance
-        for (size_t i = 0; i < workingSet.size(); ++i) {
-            int indexV_NL = workingSet[i]; // NL = nodeList
-            Node* v = &nodeList[indexV_NL];
-            double dist = distanceDij(indexU_NL, indexV_NL);
-            // is ther an alternative path which is shorter?
-            double alternative = distanceU + dist;
-            if (alternative < v->distance)  {
-                v->distance         = alternative;
-                v->predecessorIndex = indexU_NL;
-            }
-        }
-
-    }
-    // end Djikstra Algorithm
-
-
-    // reverse assemble path
-    int e = endIndex;
-    while (1) {
-        if (e == -1) {
-            if (elementPath[0] == startIndex)// check if starting point was reached
-                break;
-            return false;
-        }
-        elementPath.insert(elementPath.begin(), e);
-
-        //Update Node
-        e = nodeList[e].predecessorIndex;
-
-    }
-    return true;
-}
-
-void toDistanceMatrix(Matrix<double> &graph)
-{
-    size_t n = graph.getN();
-
-    auto distance = [graph](size_t i, size_t j){
-        return graph.get(i,j);
-    };
-
-
-    std::vector<size_t> path;
-    for (size_t i=0; i < n; ++i) {
-        for (size_t j=i+1; j < n; ++j){
-            double d = graph.get(i,j);
-            if (!std::isinf(d))
-                continue;
-            path.clear();
-            bool ret = dijkstraAlgorithm(n, i, j, path, distance);
-            assert(ret);
-            (void)ret;
-//            cout << "(" << i << "," << j << ") d: " << d << endl;
-//            cout << "Path size: " << path.size() << endl;
-//            for (auto idx : path)
-//                cout << idx << " ";
-//            cout << endl;
-
-            d = 0;
-            for (long k=0; k < long(path.size())-1; ++k) {
-                size_t idx0 = path[k];
-                size_t idx1 = path[k+1];
-                double d0 = graph.get(idx0, idx1);
-                assert(std::isinf(d0) == false);
-                d += d0;
-            }
-
-            graph.set(i, j, d);
-            graph.set(j, i, d);
-        }
-    }
-}
-
-void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex, size_t endIndex, std::vector<size_t> &pathIdx)
-{
-
-    if (!std::isinf(graph.get(startIndex, endIndex))){
-        pathIdx.push_back(startIndex);
-        pathIdx.push_back(endIndex);
-    } else {
-        auto distance = [graph](size_t i, size_t j){
-            return graph.get(i, j);
-        };
-        bool ret = dijkstraAlgorithm(graph.getN(), startIndex, endIndex, pathIdx, distance);
-        assert(ret);
-        (void)ret;
-    }
-
-}
-
-} // end namespace snake_geometry
+

src/Snake/snake_geometry.h

-#pragma once
-#include <vector>
-#include <array>
-#include <boost/geometry.hpp>
-#include <bits/stl_set.h>
-
-namespace bg = boost::geometry;
-
-namespace snake_geometry {
-
-typedef std::array<double, 3>   Point2D;
-typedef std::array<double, 3>   Point3D;
-typedef std::array<double, 3>   GeoPoint3D;
-typedef std::array<double, 2>   GeoPoint2D;
-typedef std::vector<Point2D>    Point2DList;
-typedef std::vector<Point3D>    Point3DList;
-typedef std::vector<GeoPoint2D> GeoPoint2DList;
-typedef std::vector<GeoPoint3D> GeoPoint3DList;
-
-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;
-
-
-template<class T>
-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);
-    }
-
-    double get(size_t i, size_t j) const
-    {
-        assert(_isInitialized);
-        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);
-        assert(i < _m);
-        assert(j < _n);
-        _matrix[i*_m+j] = value;
-    }
-
-    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;
-    }
-
-    void setDimension(size_t m, size_t n)
-    {
-        setDimension(m, n, T{0});
-    }
-
-private:
-    size_t _elements;
-    size_t _m;
-    size_t _n;
-    bool   _isInitialized;
-    std::vector<T> _matrix;
-};
-
-typedef struct {
-    double width;
-    double height;
-    double angle;
-    BoostPolygon corners;
-}min_bbox_rt;
-
-void toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position, Point3D &ENUPosition);
-void fromENU(const Point3D &WGS84Reference, const Point3D &ENUPosition, GeoPoint3D &WGS84Position);
-
-void polygonCenter(const BoostPolygon &polygon, BoostPoint &center);
-void minimalBoundingBox(const BoostPolygon &polygon, min_bbox_rt &minBBox);
-void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset);
-
-void graphFromPolygon(const BoostPolygon &polygon, const BoostLineString &vertices, Matrix<double> &graph);
-void 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);
-
-
-void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree);
-void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad);
-
-bool isClockwise(const Point2DList &point_list);
-
-void toBoost(const Point2D &point, BoostPoint &boost_point);
-void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon);
-
-void fromBoost(const BoostPoint &boost_point, Point2D &point);
-void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list);
-}

src/Snake/snake_global.h

-#ifndef SNAKE_GLOBAL_H
-#define SNAKE_GLOBAL_H
-
-#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
-#  define Q_DECL_EXPORT __declspec(dllexport)
-#  define Q_DECL_IMPORT __declspec(dllimport)
-#else
-#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
-#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
-#endif
-
-#if defined(SNAKE_LIBRARY)
-#  define SNAKE_EXPORT Q_DECL_EXPORT
-#else
-#  define SNAKE_EXPORT Q_DECL_IMPORT
-#endif
-
-#endif // SNAKE_GLOBAL_H

src/Snake/snake_typedefs.h

+#pragma once
+
+#include <vector>
+#include <array>
+#include <boost/geometry.hpp>
+
+namespace bg = boost::geometry;
+namespace snake {
+
+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;
+
+}

src/Wima/Geometry/WimaPolygonArray.h

@@ -41,6 +41,12 @@ public:
         return _polygons;
     }
 
+    WimaPolygonArray &operator =(const WimaPolygonArray &other){
+        this->_polygons = other._polygons;
+        this->_dirty = true;
+        return *this;
+    }
+
 
 
 private:

src/Wima/Snake/SnakeTiles.h → src/Wima/Snake/GeoPolygonArray.h

 #pragma once
-#include "snaketile.h"
+#include "GeoTile.h"
 #include "Wima/Geometry/WimaPolygonArray.h"
 
-typedef WimaPolygonArray<SnakeTile, QVector> SnakeTiles;
+using GeoPolygonArray =  WimaPolygonArray<GeoTile, QVector>;

src/Wima/Snake/snaketile.cpp → src/Wima/Snake/GeoTile.cpp

-#include "snaketile.h"
+#include "GeoTile.h"
 
-SnakeTile::SnakeTile() : WimaAreaData()
+GeoTile::GeoTile() : WimaAreaData()
 {
 
 }
 
-SnakeTile::SnakeTile(const SnakeTile &other) : WimaAreaData()
+GeoTile::GeoTile(const GeoTile &other) : WimaAreaData()
 {
     *this = other;
 }
 
-SnakeTile &SnakeTile::operator=(const SnakeTile &other)
+GeoTile &GeoTile::operator=(const GeoTile &other)
 {
     this->assign(other);
     return *this;
 
 }
 
-void SnakeTile::assign(const SnakeTile &other)
+void GeoTile::assign(const GeoTile &other)
 {
     WimaAreaData::assign(other);
 }

src/Wima/Snake/GeoTile.h

+#pragma once
+
+#include "Wima/Geometry/WimaAreaData.h"
+
+class GeoTile : public WimaAreaData
+{
+public:
+    GeoTile();
+    GeoTile(const GeoTile&other);
+
+    QString type() const {return "Tile";}
+    GeoTile* Clone() const {return new GeoTile(*this);}
+
+    GeoTile& operator=(const GeoTile &other);
+
+protected:
+    void assign(const GeoTile &other);
+};
+

src/Wima/Snake/SnakeTilesLocal.h → src/Wima/Snake/PolygonArray.h

@@ -5,4 +5,4 @@
 #include "Wima/Geometry/WimaPolygonArray.h"
 
 namespace  MsgGroups = ROSBridge::MessageGroups;
-typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> SnakeTilesLocal;
+typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> PolygonArray;

src/Wima/Snake/SnakeWorker.cc

@@ -118,28 +118,28 @@ void SnakeWorker::run()
         const auto &cps = scenario.getTileCenterPoints();
         for ( unsigned int i=0; i < tiles.size(); ++i ) {
             const auto &tile = tiles[i];
-            SnakeTile Tile;
+            GeoTile geoTile;
             for ( const auto &vertex : tile) {
                 QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
-                Tile.append(QVertex);
+                geoTile.append(QVertex);
             }
             const auto &centerPoint = cps[i];
-            QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
-            Tile.setCenter(QCenterPoint);
-            _result.tiles.polygons().append(Tile);
-            _result.tileCenterPoints.append(QVariant::fromValue(QCenterPoint));
+            QGeoCoordinate c(centerPoint[0], centerPoint[1], centerPoint[2]);
+            geoTile.setCenter(c);
+            _result.tiles.polygons().append(geoTile);
+            _result.tileCenterPoints.append(QVariant::fromValue(c));
          }
 
         // Get local tiles.
         const auto &tilesENU = scenario.getTilesENU();
         for ( unsigned int i=0; i < tilesENU.size(); ++i ) {
             const auto &tile = tilesENU[i];
-            Polygon2D Tile;
+            Polygon2D localTile;
             for ( const auto &vertex : tile.outer()) {
                 QPointF QVertex(vertex.get<0>(), vertex.get<1>());
-                Tile.path().append(QVertex);
+                localTile.path().append(QVertex);
             }
-            _result.tilesLocal.polygons().append(Tile);
+            _result.tilesLocal.polygons().append(localTile);
         }
 
     }

src/Wima/Snake/SnakeWorker.h

@@ -7,22 +7,22 @@
 #include <QGeoCoordinate>
 #include <vector>
 
-#include "SnakeTiles.h"
-#include "SnakeTilesLocal.h"
+#include "GeoPolygonArray.h"
+#include "PolygonArray.h"
 
 
 typedef QList<QVariant> QVariantList;
 
 typedef struct Result{
     QVector<QGeoCoordinate> waypoints;
-    SnakeTiles              tiles;
-    SnakeTilesLocal         tilesLocal;
+    GeoPolygonArray         tiles;
+    PolygonArray            tilesLocal;
     QVariantList            tileCenterPoints;
     QGeoCoordinate          origin;
     QVector<unsigned long>  arrivalPathIdx;
     QVector<unsigned long>  returnPathIdx;
     bool                    success;
-    QString                 errorMessage;    
+    QString                 errorMessage;
 
     void clear();
 }WorkerResult_t;

src/Wima/Snake/snaketile.h

-#pragma once
-
-#include "Wima/Geometry/WimaAreaData.h"
-
-class SnakeTile : public WimaAreaData
-{
-public:
-    SnakeTile();
-    SnakeTile(const SnakeTile&other);
-
-    QString type() const {return "SnakeTile";}
-    SnakeTile* Clone() const {return new SnakeTile(*this);}
-
-    SnakeTile& operator=(const SnakeTile &other);
-
-protected:
-    void assign(const SnakeTile &other);
-};
-

src/Wima/WimaController.cc

@@ -112,7 +112,7 @@ WimaController::WimaController(QObject *parent)
     _eventTimer.start(EVENT_TIMER_INTERVAL);
 
     // Snake Worker Thread.
-    connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults);
+    connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_updateSnakeData);
     connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
     connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
 
@@ -818,13 +818,12 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress)
     }
 }
 
-void WimaController::_snakeStoreWorkerResults()
-{
+void WimaController::_updateSnakeData()
+{    
+    _setSnakeCalcInProgress(false);
     auto start = std::chrono::high_resolution_clock::now();
     _snakeManager.clear();
-
-    const WorkerResult_t &r = _snakeWorker.result();
-    _setSnakeCalcInProgress(false);
+    const auto& r = _snakeWorker.result();
     if (!r.success) {
         //qgcApp()->showMessage(r.errorMessage);
         return;
@@ -841,18 +840,43 @@ void WimaController::_snakeStoreWorkerResults()
     unsigned  long endIdx = r.returnPathIdx.first();
     for (unsigned long i = startIdx; i <= endIdx; ++i) {
         _snakeManager.push_back(r.waypoints[int(i)]);
-    }
+    }    
+    auto end = std::chrono::high_resolution_clock::now();
+    double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: push_back waypoints execution time: " << duration << " ms.";
 
+
+    start = std::chrono::high_resolution_clock::now();
     _snakeManager.update();
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: _snakeManager.update(); execution time: " << duration << " ms.";
+
 
+    start = std::chrono::high_resolution_clock::now();
     emit missionItemsChanged();
     emit currentMissionItemsChanged();
     emit currentWaypointPathChanged();
     emit waypointPathChanged();
-
-    auto end = std::chrono::high_resolution_clock::now();
-    double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
-    qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: gui update execution time: " << duration << " ms.";
+
+    start = std::chrono::high_resolution_clock::now();
+    _snakeOrigin            = r.origin;
+    _snakeTileCenterPoints  = r.tileCenterPoints;
+    _snakeTiles             = r.tiles;
+    _snakeTilesLocal        = r.tilesLocal;
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: tiles copy execution time: " << duration << " ms.";
+
+    start = std::chrono::high_resolution_clock::now();
+    emit snakeTilesChanged();
+    emit snakeTileCenterPointsChanged();
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: gui update 2 execution time: " << duration << " ms.";
 }
 
 void WimaController::_initStartSnakeWorker()

src/Wima/WimaController.h

@@ -29,8 +29,8 @@
 
 #include "snake.h"
 #include "Snake/SnakeWorker.h"
-#include "Snake/SnakeTiles.h"
-#include "Snake/SnakeTilesLocal.h"
+#include "Snake/GeoPolygonArray.h"
+#include "Snake/PolygonArray.h"
 #include "Geometry/GeoPoint3D.h"
 #include "Snake/QNemoProgress.h"
 #include "Snake/QNemoHeartbeat.h"
@@ -338,7 +338,7 @@ private slots:
     void    _smartRTLCleanUp                    (bool flying);
     // Snake.
     void _setSnakeCalcInProgress             (bool inProgress);
-    void _snakeStoreWorkerResults            ();
+    void _updateSnakeData                    ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
     // Periodic tasks.
@@ -402,9 +402,9 @@ private:
     // Snake
     bool                    _snakeCalcInProgress;
     SnakeWorker             _snakeWorker;
-    GeoPoint              _snakeOrigin;
-    SnakeTiles              _snakeTiles; // tiles
-    SnakeTilesLocal         _snakeTilesLocal; // tiles local coordinate system
+    GeoPoint                _snakeOrigin;
+    GeoPolygonArray         _snakeTiles; // tiles
+    PolygonArray            _snakeTilesLocal; // tiles local coordinate system
     QVariantList            _snakeTileCenterPoints;
     QNemoProgress           _nemoProgress; // measurement progress
     QNemoHeartbeat          _nemoHeartbeat; // measurement progress

src/comm/ros_bridge/include/RosBridgeClient.h

@@ -5,10 +5,6 @@
 *      Author: Poom Pianpak
 */
 
-
-#ifndef ROSBRIDGECPP_ROSBRIDGE_WS_CLIENT_HPP_
-#define ROSBRIDGECPP_ROSBRIDGE_WS_CLIENT_HPP_
-
 #include "Simple-WebSocket-Server/client_ws.hpp"
 
 #include "rapidjson/include/rapidjson/document.h"