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

moved class route in seperate folder

Browse Source
This commit is contained in:
kleiax
2023-08-09 14:33:51 +02:00
parent 6b8b56fc1a
commit a00ee4080c
2 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/route.cpp
-181
View File
@@ -1,181 +0,0 @@
/**
* @file route.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Implements the class Route and Point
* @version 0.1
* @date 2022-01-31
*
* @copyright Copyright (c) 2022
*
*/
#include "route.h"
Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat;
this->coordinates.lon = lon;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat / 10000000.0;
this->coordinates.lon = lon / 10000000.0;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates = coords;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, bool imported) {
this->coordinates = coords;
if (imported)
this->init(UINT32_MAX, 0);
else
this->init(0, 0);
}
Point::Point() {
this->coordinates.lat = 0;
this->coordinates.lon = 0;
this->init(0, 0);
}
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 = (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::distanceTo(const Point &point) const {
return this->distanceTo(point.getCoordinates());
}
int16_t 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);
int16_t res = (int16_t) atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
// if (res < 0)
// res += 360;
return res;
}
int16_t Point::courseTo(const Point &point) const {
return this->courseTo(point.getCoordinates());
}
void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) {
this->creationTime = creationTime;
if (horizontalAccuracy == UINT32_MAX)
this->accuracy = Accuracy::imported;
else if (horizontalAccuracy > 9999)
this->accuracy = Accuracy::fourDigOfCM;
else if (horizontalAccuracy > 999)
this->accuracy = Accuracy::threeDigOfCM;
else if (horizontalAccuracy > 99)
this->accuracy = Accuracy::twoDigOfCM;
else if (horizontalAccuracy > 1)
this->accuracy = Accuracy::oneDigOfCM;
else
this->accuracy = Accuracy::none;
}
Route::Route() {
}
void Route::addPointToRoute(Point point) {
this->points.push_back(point);
}
void Route::clear() {
this->points.clear();
this->currentPoint = 0;
this->started = false;
}
Point Route::startRoute() {
if (this->points.size() < 1) {
this->started = false;
return Point();
}
this->it = this->points.begin();
this->currentPoint = 1;
this->started = true;
return *this->it;
}
Point Route::endRoute() {
if (this->points.size() < 1) {
this->started = false;
return Point();
}
this->it = this->points.end();
// TODO: Understand why i have to decrement the iterator first to get realy the last element.
this->it--;
this->currentPoint = this->points.size();
this->started = true;
return *this->it;
}
Point Route::getNextPoint() {
if (!this->started)
return Point();
if (this->it != --this->points.end()) {
this->it++;
this->currentPoint++;
return *this->it;
// } else if (this->it == this->points.end() && this->currentPoint != this->points.size()) {
// this->currentPoint++;
// return *this->it;
}
return Point();
}
Point Route::getPreviousPoint() {
if (!this->started)
return Point();
if (this->it != this->points.begin()) {
this->it--;
this->currentPoint--;
return *this->it;
} else {
return Point();
}
}
RouteInfo Route::getRouteInfo() {
RouteInfo info;
info.totalPoints = this->points.size();
info.currentPoint = this->currentPoint;
return info;
}
lib/Navigation/route.h
-238
View File
@@ -1,238 +0,0 @@
/**
* @file route.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the class Point and Route
* @version 0.1
* @date 2022-01-31
*
* @copyright Copyright (c) 2022
*
*/
#ifndef ROUTE_H
#define ROUTE_H
#include <cstdint>
#include <list>
#include <cmath>
#define ROUTE_DEGREE_TO_RADIANT 0.01745
#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
#define ROUTE_PI 3.14159265358979323846
/**
* @brief A to handle points on the earth
*
* The points inherits latidue and longitude as doubles
*
*/
class Point{
public:
/**
* @brief Hold the data longitude and latitude
*
*/
struct Coordinates {
double lon;
double lat;
bool operator==(const Coordinates rhs) const {
return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon );
}
};
/**
* @brief The Accuracy is set by the constructor
*
*/
enum Accuracy {
none,
fourDigOfCM,
threeDigOfCM,
twoDigOfCM,
oneDigOfCM,
imported
};
/**
* @brief Construct a new Point object
*
* @param lat latitude
* @param lon longitude
* @param horizontalAccuracy mm
* @param coords Coordinates
* @param imported if true than highest accuracy
*/
Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(Coordinates coords, bool imported);
Point();
/**
* @brief Checks if to points are equal.
*
* @param rhs
* @return true
* @return false
*/
bool operator==(const Point& rhs) const;
/**
* @brief Checks if the point is initalized.
*
* @return true
* @return false
*/
bool isInit() const { return this->coordinates.lat + this->coordinates.lon; }
/**
* @brief Checks if the point is valid.
*
* If the accuracy is higher than zero, true will be returned.
*
* @return true
* @return false
*/
bool isValid() const { return (this->accuracy > 0) ? true : false; }
/**
* @brief Calculates the distance between to points.
*
* @param point
* @return double meter
*/
double distanceTo(const Coordinates& point) const;
double distanceTo(const Point& point) const;
/**
* @brief Calculates the course to an other point.
*
* @param point
* @return int16_t degree
*/
int16_t courseTo(const Coordinates& point) const;
int16_t courseTo(const Point& point) const;
uint32_t getCreationTime() const { return this->creationTime; }
double getLongitude() const { return this->coordinates.lon; }
double getLatitude() const { return this->coordinates.lat; }
Coordinates getCoordinates() const { return this->coordinates; }
/**
* @brief Get the Accuracy object
*
* The higher the value, the greater the accuracy.
* You can check it by Accuracy.
*
* @return Accuracy
*/
Accuracy getAccuracy() { return this->accuracy; }
private:
void init(uint32_t horizontalAccuracy, uint32_t creationTime);
Accuracy accuracy = Accuracy::none;
Coordinates coordinates;
uint32_t creationTime = 0;
};
/**
* @brief Holds some route information
*
*/
struct RouteInfo{
/**
* @brief Selected number of Points
*/
uint16_t currentPoint;
/**
* @brief Total points stored in route
*/
uint16_t totalPoints;
};
/**
* @brief A class to manage multiple points
*
* The list of point presents a route which can be driven
*/
class Route {
public:
/**
* @brief Construct a new Route object.
*/
Route();
/**
* @brief Adds a point to the list.
*
* @param point
*/
void addPointToRoute(Point point);
/**
* @brief Delete all points.
*
*/
void clear();
/**
* @brief Select the first point as target.
*
* @return Point
*/
Point startRoute();
/**
* @brief Select the last point as target.
*
* @return Point
*/
Point endRoute();
/**
* @brief Get the next point and set it as target.
*
* @return Point is zero if there are no more Points.
*/
Point getNextPoint();
/**
* @brief Get the previous point and set it as target.
*
* @return Point
*/
Point getPreviousPoint();
/**
* @brief Get the Route Info object
*
* @return RouteInfo
*/
RouteInfo getRouteInfo();
/**
* @brief Get the Started object
*
* The Route will be marked as started when startRoute or
* endRoute has been called.
*
* @return true
* @return false
*/
bool getStarted() const { return this->started; }
private:
uint16_t currentPoint = 0;
bool started = false;
std::list<Point> points;
std::list<Point>::iterator it;
};
#endif // ROUTE_H