97 lines
2.4 KiB
C++
97 lines
2.4 KiB
C++
/**
|
|
* @file route.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Implements the class Route
|
|
* @version 0.1
|
|
* @date 2022-01-31
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#include "route.h"
|
|
|
|
Point::Point(double lat, double lon, uint32_t horizontalAccuracy) {
|
|
this->lat = lat;
|
|
this->lon = lon;
|
|
this->init(horizontalAccuracy);
|
|
}
|
|
|
|
Point::Point() {
|
|
this->lat = 0;
|
|
this->lon = 0;
|
|
this->init(0);
|
|
}
|
|
|
|
double Point::distanceTo(const Point &point) const {
|
|
// 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 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);
|
|
|
|
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 res = atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
|
|
|
|
// if (res < 0)
|
|
// res += 360.0;
|
|
|
|
return res;
|
|
}
|
|
|
|
void Point::init(uint32_t horizontalAccuracy) {
|
|
this->creationTime = millis();
|
|
if (horizontalAccuracy > 9999)
|
|
this->accuracy = PointAccuracy::fourDigOfCM;
|
|
else if (horizontalAccuracy > 999)
|
|
this->accuracy = PointAccuracy::threeDigOfCM;
|
|
else if (horizontalAccuracy > 99)
|
|
this->accuracy = PointAccuracy::twoDigOfCM;
|
|
|
|
}
|
|
|
|
Route::Route() {
|
|
|
|
}
|
|
|
|
void Route::addPointToRoute(Point point) {
|
|
this->points.push_back(point);
|
|
}
|
|
|
|
Point Route::startRoute() {
|
|
if (this->points.size() < 1)
|
|
return Point(0, 0);
|
|
this->it = this->points.begin();
|
|
this->currentPoint = 1;
|
|
return *this->it;
|
|
}
|
|
|
|
Point Route::getNextPoint() {
|
|
if (this->it != this->points.end()) {
|
|
this->it++;
|
|
this->currentPoint++;
|
|
return *this->it;
|
|
}
|
|
else
|
|
return Point(0, 0);
|
|
}
|
|
|
|
RouteInfo Route::getRouteInfo() {
|
|
RouteInfo info;
|
|
info.totalPoints = this->points.size();
|
|
info.currentPoint = this->currentPoint;
|
|
return info;
|
|
}
|