kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

refactor class route add a struct to hold coords

Browse Source
This commit is contained in:
kleiax
2022-12-27 13:11:03 +01:00
parent a09cfe9b04
commit b9840bccf8
2 changed files with 60 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/route.cpp
+39 -12
View File
@@ -12,36 +12,59 @@
#include "route.h"
Point::Point(double lat, double lon, uint32_t horizontalAccuracy) {
this->lat = lat;
this->lon = lon;
this->coordinates.lat = lat;
this->coordinates.lon = lon;
this->init(horizontalAccuracy);
}
Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy) {
this->coordinates.lat = lat / 10000000.0;
this->coordinates.lon = lon / 10000000.0;
this->init(horizontalAccuracy);
}
Point::Point(Coordinates coords, uint32_t horizontalAccuracy) {
this->coordinates = coords;
this->init(horizontalAccuracy);
}
Point::Point() {
this->lat = 0;
this->lon = 0;
this->coordinates.lat = 0;
this->coordinates.lon = 0;
this->init(0);
}
double Point::distanceTo(const Point &point) const {
// distance = sqrt(dx * dx + dy * dy)
bool Point::operator==(const Point& rhs) const {
return this->coordinates == rhs.getCoordinates();
}
// distance = sqrt(dx * dx + dy * dy)
// mit distance: Entfernung in km
// dx = 111.3 * cos(lat) * (lon1 - lon2)
// lat = (lat1 + lat2) / 2 * 0.01745
// dy = 111.3 * (lat1 - lat2)
// lat1, lat2, lon1, lon2: Breite, Länge in Grad
double Point::distanceTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double lat = (this->lat + point.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (this->lat - point.lat);
double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (this->lon - point.lon);
double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
return sqrt(dx * dx + dy * dy);
}
double Point::courseTo(const Point &point) const {
double phi = log( tan(point.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(this->lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
double lon = (this->lon * ROUTE_DEGREE_TO_RADIANT - point.lon * ROUTE_DEGREE_TO_RADIANT);
double Point::distanceTo(const Point &point) const {
return this->distanceTo(point.getCoordinates());
}
double Point::courseTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
double res = atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
@@ -51,6 +74,10 @@ double Point::courseTo(const Point &point) const {
return res;
}
double Point::courseTo(const Point &point) const {
return this->courseTo(point.getCoordinates());
}
void Point::init(uint32_t horizontalAccuracy) {
this->creationTime = millis();
if (horizontalAccuracy > 9999)