#pragma once #include #include #include #include #include #include #include #include #include #include namespace bg = boost::geometry; namespace bu = boost::units; #include #include using namespace std; namespace snake { //========================================================================= // Geometry stuff. //========================================================================= typedef bg::model::point BoostPoint; // typedef std::vector BoostPointList; typedef bg::model::polygon BoostPolygon; typedef bg::model::linestring BoostLineString; typedef std::vector> Int64Matrix; typedef bg::model::box BoostBox; template 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 _matrix; }; struct BoundingBox { BoundingBox(); void clear(); double width; double height; double angle; BoostPolygon corners; }; template 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 = 0, y = 0, z = 0; auto alt = in.altitude(); alt = std::isnan(alt) ? 0 : alt; proj.Forward(in.latitude(), in.longitude(), alt, x, y, z); out.set<0>(x); out.set<1>(y); (void)z; } template 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 = 0, lon = 0, alt = 0; proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt); out.setLatitude(lat); out.setLongitude(lon); out.setAltitude(alt); } template class Container> void areaToEnu(const GeoPoint &origin, const Container &in, BoostPolygon &out) { for (auto vertex : in) { BoostPoint p; toENU(origin, vertex, p); out.outer().push_back(p); } bg::correct(out); } template class Container> void areaFromEnu(const GeoPoint &origin, BoostPolygon &in, const Container &out) { for (auto vertex : in.outer()) { GeoPoint p; fromENU(origin, vertex, p); out.push_back(p); } } void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er); bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox); void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset); void graphFromPolygon(const BoostPolygon &polygon, const BoostLineString &vertices, Matrix &graph); void toDistanceMatrix(Matrix &graph); bool dijkstraAlgorithm( const size_t numElements, size_t startIndex, size_t endIndex, std::vector &elementPath, std::function distanceDij); void shortestPathFromGraph(const Matrix &graph, size_t startIndex, size_t endIndex, std::vector &pathIdx); typedef bu::quantity Length; typedef bu::quantity Area; typedef bu::quantity Angle; typedef bu::quantity Radian; typedef bu::quantity Degree; bool joinedArea(const std::vector &areas, BoostPolygon &jArea); bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea, const BoostPolygon &corridor, BoostPolygon &jArea, std::string &errorString); bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth, Area minTileArea, std::vector &tiles, BoundingBox &bbox, std::string &errorString); using Transects = vector; using Progress = vector; using Route = BoostLineString; bool transectsFromScenario(Length distance, Length minLength, Angle angle, const BoostPolygon &mArea, const std::vector &tiles, const Progress &p, Transects &t, string &errorString); struct TransectInfo { TransectInfo(size_t n, bool r) : index(n), reversed(r) {} size_t index; bool reversed; }; bool route(const BoostPolygon &area, const Transects &transects, std::vector &transectInfo, Route &route, string &errorString); namespace detail { const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. } // namespace detail } // namespace snake // operator== and operator!= for boost point namespace boost { namespace geometry { namespace model { bool operator==(snake::BoostPoint p1, snake::BoostPoint p2); bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2); } // namespace model } // namespace geometry } // namespace boost