Newer
Older
#pragma once
#include <vector>
#include <string>
#include <array>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
using namespace std;
namespace snake {
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
//=========================================================================
// 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;}
void set(size_t i, size_t j, const T &value)
{
assert(_isInitialized);
assert(i < _m);
assert(j < _n);
_matrix[i*_m+j] = value;
}
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
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 ¢er);
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);
}
}
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);
const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies.