145 lines
3.9 KiB
C++
145 lines
3.9 KiB
C++
/**
|
|
* @file point.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief
|
|
* @version 0.1
|
|
* @date 2023-09-03
|
|
*
|
|
* @copyright Copyright (c) 2023
|
|
*
|
|
*/
|
|
|
|
#include "point.h"
|
|
|
|
Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime)
|
|
: coordinates{lat, lon}
|
|
{
|
|
this->init(horizontalAccuracy, creationTime);
|
|
}
|
|
|
|
Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime)
|
|
: coordinates{lat / 10000000.0, lon / 10000000.0}
|
|
{
|
|
this->init(horizontalAccuracy, creationTime);
|
|
}
|
|
|
|
Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime)
|
|
: 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();
|
|
}
|
|
|
|
double Point::distanceTo(const Coordinates &point) const
|
|
{
|
|
// Old implementation
|
|
// const Coordinates begin = this->coordinates;
|
|
// const Coordinates end = point;
|
|
|
|
// const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
|
|
// const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
|
|
// const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
|
|
|
|
// return sqrt(dx * dx + dy * dy);
|
|
|
|
double latStart = this->toRad(this->coordinates.lat);
|
|
double lonStart = this->toRad(this->coordinates.lon);
|
|
double latEnd = this->toRad(point.lat);
|
|
double lonEnd = this->toRad(point.lon);
|
|
|
|
const double earthRadius = 6371.0; //km
|
|
double cartesianX_1 = cos(latStart) * lonStart * earthRadius;
|
|
double cartesianY_1 = latStart * earthRadius;
|
|
double cartesianX_2 = cos(latEnd) * lonEnd * earthRadius;
|
|
double cartesianY_2 = latEnd * earthRadius;
|
|
|
|
return sqrt(pow(cartesianX_2 - cartesianX_1, 2)
|
|
+ pow(cartesianY_2 - cartesianY_1, 2));
|
|
}
|
|
|
|
double Point::distanceTo(const Point &point) const
|
|
{
|
|
return this->distanceTo(point.getCoordinates());
|
|
}
|
|
|
|
int16_t Point::courseTo(const Coordinates &point) const
|
|
{
|
|
// const Coordinates begin = this->coordinates;
|
|
// const Coordinates end = point;
|
|
|
|
// const double phi = log(tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4));
|
|
// const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
|
|
|
|
// return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
|
|
|
|
double latStart = this->toRad(this->coordinates.lat);
|
|
double lonStart = this->toRad(this->coordinates.lon);
|
|
double latEnd = this->toRad(point.lat);
|
|
double lonEnd = this->toRad(point.lon);
|
|
|
|
double y = sin(lonEnd - lonStart) * cos(latEnd);
|
|
double x = cos(latStart) * sin(latEnd) - sin(latStart) * cos(latEnd) * cos(lonEnd - lonStart);
|
|
double theta = atan2(y, x);
|
|
|
|
return static_cast<int16_t>(this->toDeg(theta)) % 360;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|