Commit 670dffae authored by Valentin Platzgummer's avatar Valentin Platzgummer

geometry.h toENU nan issue solved

parent 44b635f9
......@@ -222,7 +222,7 @@ void AreaData::intersection(bool showError) {
geometry::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
geometry::FPolygon measurementAreaENU;
geometry::areaToEnu(origin, measurementArea->pathModel(),
measurementAreaENU);
measurementAreaENU);
// do intersection
std::deque<geometry::FPolygon> outputENU;
......@@ -248,8 +248,9 @@ void AreaData::intersection(bool showError) {
auto large = std::move(outputENU[0]);
geometry::FPolygon small;
while (!bg::covered_by(large, safeAreaENU)) {
geometry::offsetPolygon(large, small, -0.1);
geometry::offsetPolygon(large, small, -0.01);
large = std::move(small);
qDebug() << "shrink";
}
// Check if result is different from input.
......
......@@ -18,11 +18,9 @@
namespace bg = boost::geometry;
namespace bu = boost::units;
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include "QGCQGeoCoordinate.h"
#include "QmlObjectListModel.h"
#include <QPointF>
namespace geometry {
//=========================================================================
......@@ -109,34 +107,6 @@ static constexpr int earth_radius = 6371000; // meters (m)
static constexpr double epsilon =
std::numeric_limits<double>::epsilon(); // meters (m)
template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
double lat_rad = in.latitude() * M_PI / 180;
double lon_rad = in.longitude() * M_PI / 180;
double ref_lon_rad = origin.longitude() * M_PI / 180;
double ref_lat_rad = origin.latitude() * M_PI / 180;
double sin_lat = std::sin(lat_rad);
double cos_lat = std::cos(lat_rad);
double cos_d_lon = std::cos(lon_rad - ref_lon_rad);
double ref_sin_lat = std::sin(ref_lat_rad);
double ref_cos_lat = std::cos(ref_lat_rad);
double c =
std::acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
double k = (std::fabs(c) < epsilon) ? 1.0 : (c / std::sin(c));
double x = k * cos_lat * std::sin(lon_rad - ref_lon_rad) * earth_radius;
double y = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) *
earth_radius;
out.set<0>(x);
out.set<1>(y);
}
template <class GeoPoint1, class GeoPoint2, class Point>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
......@@ -155,7 +125,10 @@ void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
double ref_sin_lat = sin(ref_lat_rad);
double ref_cos_lat = cos(ref_lat_rad);
double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
double value = ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon;
value = value > 1 ? 1 : value;
value = value < -1 ? -1 : value;
double c = acos(value);
double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));
double x = k * cos_lat * sin(lon_rad - ref_lon_rad) * earth_radius;
......@@ -166,13 +139,13 @@ void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
out.setY(y);
}
template <class GeoPoint>
void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
template <class GeoPoint1, class Point, class GeoPoint2>
void fromENU(const GeoPoint1 &origin, const Point &in, GeoPoint2 &out) {
using namespace std;
double x_rad = in.get<0>() / earth_radius;
double y_rad = in.get<1>() / earth_radius;
double x_rad = in.x() / earth_radius;
double y_rad = in.y() / earth_radius;
double c = sqrt(y_rad * y_rad + x_rad * x_rad);
double sin_c = sin(c);
double cos_c = cos(c);
......@@ -187,7 +160,11 @@ void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
double lon_rad;
if (fabs(c) > epsilon) {
lat_rad = asin(cos_c * ref_sin_lat + (y_rad * sin_c * ref_cos_lat) / c);
double v1 = cos_c * ref_sin_lat + (y_rad * sin_c * ref_cos_lat) / c;
v1 = v1 > 1 ? 1 : v1;
v1 = v1 < -1 ? -1 : v1;
lat_rad = asin(v1);
lon_rad =
(ref_lon_rad + atan2(x_rad * sin_c, c * ref_cos_lat * cos_c -
y_rad * ref_sin_lat * sin_c));
......@@ -202,6 +179,22 @@ void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
out.setAltitude(origin.altitude());
}
template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
QPointF p;
toENU(origin, in, p);
out.set<0>(p.x());
out.set<1>(p.y());
}
template <class GeoPoint1, class GeoPoint2>
void fromENU(const GeoPoint1 &origin, const FPoint &in, GeoPoint2 &out) {
QPointF p(in.get<0>(), in.get<1>());
fromENU(origin, p, out);
}
template <class GeoPoint, class Container1, class Container2>
void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) {
for (auto &vertex : in) {
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment