CSWorker.cpp

#include "CSWorker.h"
// Wima
#define CLIPPER_SCALE 10000
#include "clipper/clipper.hpp"
template <int k> ClipperLib::cInt get(ClipperLib::IntPoint &p);
#include "Geometry/GenericCircle.h"
// std
#include <chrono>
// Qt
#include <QDebug>

template <> ClipperLib::cInt get<0>(ClipperLib::IntPoint &p) { return p.X; }
template <> ClipperLib::cInt get<1>(ClipperLib::IntPoint &p) { return p.Y; }

CSWorker::CSWorker(QObject *parent)
    : QThread(parent), _deltaR(2 * bu::si::meter),
      _deltaAlpha(3 * bu::degree::degree), _minLength(10 * bu::si::meter),
      _calculating(false), _stop(false), _restart(false) {}

CSWorker::~CSWorker() {
  this->_stop = true;
  Lock lk(this->_mutex);
  this->_restart = true;
  this->_cv.notify_one();
  lk.unlock();
  this->wait();
}

bool CSWorker::calculating() { return this->_calculating; }

void CSWorker::update(const QList<QGeoCoordinate> &polygon,
                      const QGeoCoordinate &origin, snake::Length deltaR,
                      snake::Length minLength, snake::Angle deltaAlpha) {
  // Sample input.
  Lock lk(this->_mutex);
  this->_polygon = polygon;
  this->_origin = origin;
  this->_deltaR = deltaR;
  this->_deltaAlpha = deltaAlpha;
  this->_minLength = minLength;
  lk.unlock();

  if (!this->isRunning()) {
    this->start();
  } else {
    Lock lk(this->_mutex);
    this->_restart = true;
    this->_cv.notify_one();
  }
}

void CSWorker::run() {
  qWarning() << "CSWorker::run(): thread start.";
  while (!this->_stop) {
    // Copy input.
    Lock lk(this->_mutex);
    const auto polygon = this->_polygon;
    const auto origin = this->_origin;
    const auto deltaR = this->_deltaR;
    const auto deltaAlpha = this->_deltaAlpha;
    const auto minLength = this->_minLength;
    lk.unlock();
    // Check preconitions
    if (polygon.size() >= 3) {
#ifdef DEBUG_CIRCULAR_SURVEY
      qWarning() << "CSWorker::run(): calculation "
                    "started.";
#endif
#ifdef SHOW_CIRCULAR_SURVEY_TIME
      const auto start = std::chrono::high_resolution_clock::now();
#endif
      using namespace boost::units;
      this->_calculating = true;
      emit calculatingChanged();
      // Convert geo polygon to ENU polygon.
      snake::BoostPolygon polygonENU;
      snake::BoostPoint originENU{0, 0};
      snake::areaToEnu(origin, polygon, polygonENU);
      std::string error;
      // Check validity.
      if (!bg::is_valid(polygonENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
        qWarning() << "CSWorker::run(): "
                      "invalid polygon.";
        qWarning() << error.c_str();
#endif
      } else {
        // Calculate polygon distances and angles.
        std::vector<snake::Length> distances;
        distances.reserve(polygonENU.outer().size());
        std::vector<snake::Angle> angles;
        angles.reserve(polygonENU.outer().size());
#ifdef DEBUG_CIRCULAR_SURVEY
        qWarning() << "CSWorker::run():";
#endif
        for (const auto &p : polygonENU.outer()) {
          snake::Length distance = bg::distance(originENU, p) * si::meter;
          distances.push_back(distance);
          snake::Angle alpha =
              (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
          alpha =
              alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
          angles.push_back(alpha);
#ifdef DEBUG_CIRCULAR_SURVEY
          qWarning() << "distances, angles, coordinates:";
          qWarning() << to_string(distance).c_str();
          qWarning() << to_string(snake::Degree(alpha)).c_str();
          qWarning() << "x = " << p.get<0>() << "y = " << p.get<1>();
#endif
        }

        auto rMin = deltaR; // minimal circle radius
        snake::Angle alpha1(0 * degree::degree);
        snake::Angle alpha2(360 * degree::degree);
        // Determine r_min by successive approximation
        if (!bg::within(originENU, polygonENU)) {
          rMin = bg::distance(originENU, polygonENU) * si::meter;
        }

        auto rMax =
            (*std::max_element(distances.begin(),
                               distances.end())); // maximal circle radius

        // Scale parameters and coordinates.
        const auto rMinScaled =
            ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
        const auto deltaRScaled =
            ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
        auto originScaled = ClipperLib::IntPoint{
            ClipperLib::cInt(std::round(originENU.get<0>())),
            ClipperLib::cInt(std::round(originENU.get<1>()))};

#ifdef SHOW_CIRCULAR_SURVEY_TIME
        auto s1 = std::chrono::high_resolution_clock::now();
#endif
        // Generate circle sectors.
        auto rScaled = rMinScaled;
        const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
        vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
        const auto nSectors =
            long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
#ifdef DEBUG_CIRCULAR_SURVEY
        qWarning() << "CSWorker::run(): sector parameres:";
        qWarning() << "alpha1: " << to_string(snake::Degree(alpha1)).c_str();
        qWarning() << "alpha2: " << to_string(snake::Degree(alpha2)).c_str();
        qWarning() << "n: "
                   << to_string((alpha2 - alpha1) / deltaAlpha).c_str();
        qWarning() << "nSectors: " << nSectors;
        qWarning() << "rMin: " << to_string(rMin).c_str();
        qWarning() << "rMax: " << to_string(rMax).c_str();
        qWarning() << "nTran: " << nTran;
#endif
        using ClipperCircle =
            GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
        for (auto &sector : sectors) {
          ClipperCircle circle(rScaled, originScaled);
          approximate(circle, nSectors, sector);
          rScaled += deltaRScaled;
        }
        // Clip sectors to polygonENU.
        ClipperLib::Path polygonClipper;
        auto &outer = polygonENU.outer();
        polygonClipper.reserve(outer.size() - 1);
        for (auto it = outer.begin(); it < outer.end() - 1; ++it) {
          auto x = ClipperLib::cInt(std::round(it->get<0>() * CLIPPER_SCALE));
          auto y = ClipperLib::cInt(std::round(it->get<1>() * CLIPPER_SCALE));
          polygonClipper.push_back(ClipperLib::IntPoint{x, y});
        }
        ClipperLib::Clipper clipper;
        clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
        clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
        ClipperLib::PolyTree transectsClipper;
        clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
                        ClipperLib::pftNonZero, ClipperLib::pftNonZero);

        // Extract transects from  PolyTree and convert them to
        // BoostLineString
        snake::Transects transectsENU;
        for (const auto &child : transectsClipper.Childs) {
          snake::BoostLineString transect;
          transect.reserve(child->Contour.size());
          for (const auto &vertex : child->Contour) {
            auto x = static_cast<double>(vertex.X) / CLIPPER_SCALE;
            auto y = static_cast<double>(vertex.Y) / CLIPPER_SCALE;
            transect.push_back(snake::BoostPoint(x, y));
          }
          transectsENU.push_back(transect);
        }
        // Join sectors which where slit due to clipping.
        const double th = 0.01;
        for (auto ito = transectsENU.begin(); ito < transectsENU.end(); ++ito) {
          for (auto iti = ito + 1; iti < transectsENU.end(); ++iti) {
            auto dist1 = bg::distance(ito->front(), iti->front());
            if (dist1 < th) {
              snake::BoostLineString temp;
              for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
                temp.push_back(*it);
              }
              temp.insert(temp.end(), ito->begin(), ito->end());
              *ito = temp;
              transectsENU.erase(iti);
              break;
            }
            auto dist2 = bg::distance(ito->front(), iti->back());
            if (dist2 < th) {
              snake::BoostLineString temp;
              temp.insert(temp.end(), iti->begin(), iti->end());
              temp.insert(temp.end(), ito->begin(), ito->end());
              *ito = temp;
              transectsENU.erase(iti);
              break;
            }
            auto dist3 = bg::distance(ito->back(), iti->front());
            if (dist3 < th) {
              snake::BoostLineString temp;
              temp.insert(temp.end(), ito->begin(), ito->end());
              temp.insert(temp.end(), iti->begin(), iti->end());
              *ito = temp;
              transectsENU.erase(iti);
              break;
            }
            auto dist4 = bg::distance(ito->back(), iti->back());
            if (dist4 < th) {
              snake::BoostLineString temp;
              temp.insert(temp.end(), ito->begin(), ito->end());
              for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
                temp.push_back(*it);
              }
              *ito = temp;
              transectsENU.erase(iti);
              break;
            }
          }
        }
        // Remove short transects
        for (auto it = transectsENU.begin(); it < transectsENU.end();) {
          if (bg::length(*it) < minLength.value()) {
            it = transectsENU.erase(it);
          } else {
            ++it;
          }
        }
        // Move transect with min. distance to the front.
        auto minDist = std::numeric_limits<double>::max();
        auto minIt = transectsENU.begin();
        bool reverse = false;
        for (auto it = transectsENU.begin(); it < transectsENU.end(); ++it) {
          auto distFront = bg::distance(originENU, it->front());
          auto distBack = bg::distance(originENU, it->back());
          if (distFront < minDist) {
            minDist = distFront;
            minIt = it;
            reverse = false;
          }
          if (distBack < minDist) {
            minDist = distBack;
            minIt = it;
            reverse = true;
          }
        }
        // Swap and reverse (if necessary).
        if (minIt != transectsENU.begin()) {
          auto minTransect = *minIt;
          if (reverse) {
            snake::BoostLineString rev;
            for (auto it = minTransect.end() - 1; it >= minTransect.begin();
                 --it) {
              rev.push_back(*it);
            }
            minTransect = rev;
          }
          *minIt = *transectsENU.begin();
          *transectsENU.begin() = minTransect;
        }
#ifdef SHOW_CIRCULAR_SURVEY_TIME
        qWarning() << "CSWorker::run(): transect gen. time: "
                   << std::chrono::duration_cast<std::chrono::milliseconds>(
                          std::chrono::high_resolution_clock::now() - s1)
                          .count()
                   << " ms";
#endif

        if (transectsENU.size() == 0) {
#ifdef DEBUG_CIRCULAR_SURVEY
          qWarning() << "CSWorker::run(): "
                        "not able to generate transects.";
#endif
        } else if (this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
          qWarning() << "CSWorker::run(): "
                        "restart requested.";
#endif
        } else {
          // Prepare data for routing.
          std::vector<snake::TransectInfo> transectsInfo;
          snake::Route route;
          const auto routingStart = std::chrono::high_resolution_clock::now();
          const auto maxRoutingTime = std::chrono::minutes(1);
          const auto routingEnd = routingStart + maxRoutingTime;
          const auto &restart = this->_restart;
          auto stopLambda = [&restart, routingEnd] {
            bool expired =
                std::chrono::high_resolution_clock::now() > routingEnd;
            return restart || expired;
          };
          std::string errorString;
          // Route transects;
          bool success = snake::route(polygonENU, transectsENU, transectsInfo,
                                      route, stopLambda, errorString);
          if (!success && !this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
            qWarning() << "CSWorker::run(): "
                          "routing failed.";
#endif
          } else if (this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
            qWarning() << "CSWorker::run(): "
                          "restart requested.";
#endif
          } else {
            // Remove return path.
            const auto &info = transectsInfo.back();
            const auto &lastTransect = transectsENU[info.index];
            const auto &lastWaypoint =
                info.reversed ? lastTransect.front() : lastTransect.back();
            auto &wp = route.back();
            while (wp != lastWaypoint) {
              route.pop_back();
              wp = route.back();
            }
            // Convert to geo coordinates and notify main thread.
            auto pRoute = PtrRoute(new Route());
            for (const auto &vertex : route) {
              QGeoCoordinate c;
              snake::fromENU(origin, vertex, c);
              pRoute->append(c);
            }
            emit ready(pRoute);
#ifdef DEBUG_CIRCULAR_SURVEY
            qWarning() << "CSWorker::run(): "
                          "concurrent update success.";
#endif
          }
        }
      }
#ifdef SHOW_CIRCULAR_SURVEY_TIME
      qWarning() << "CSWorker::run(): execution time: "
                 << std::chrono::duration_cast<std::chrono::milliseconds>(
                        std::chrono::high_resolution_clock::now() - start)
                        .count()
                 << " ms";
#endif
      this->_calculating = false;
      emit calculatingChanged();
    }
#ifdef DEBUG_CIRCULAR_SURVEY
    else {
      qWarning() << "CSWorker::run(): preconditions failed.";
    }
#endif
    Lock lk2(this->_mutex);
    if (!this->_restart) {
      this->_cv.wait(lk2, [this] { return this->_restart.load(); });
    }
    this->_restart = false;
  }
  qWarning() << "CSWorker::run(): thread end.";
}