diff --git a/location.cpp b/location.cpp index 429abac..0a7c9eb 100644 --- a/location.cpp +++ b/location.cpp @@ -1,318 +1,318 @@ -#include "./location.h" -#include "./utils.h" - -#include - -#include -#include -#include - -using namespace std; -using namespace ApplicationUtilities; - -// WGS84 Parameters -#define WGS84_A 6378137.0 // major axis -#define WGS84_B 6356752.31424518 // minor axis -#define WGS84_F 0.0033528107 // ellipsoid flattening -#define WGS84_E 0.0818191908 // first eccentricity -#define WGS84_EP 0.0820944379 // second eccentricity - -// UTM Parameters -#define UTM_K0 0.9996 // scale factor -#define UTM_FE 500000.0 // false easting -#define UTM_FN_N 0.0 // false northing, northern hemisphere -#define UTM_FN_S 10000000.0 // false northing, southern hemisphere -#define UTM_E2 (WGS84_E * WGS84_E) // e^2 -#define UTM_E4 (UTM_E2 * UTM_E2) // e^4 -#define UTM_E6 (UTM_E4 * UTM_E2) // e^6 -#define UTM_EP2 (UTM_E2 / (1 - UTM_E2)) // e'^2 - -Location::Location() : - m_lat(0.0), - m_lon(0.0), - m_ele(0.0) -{} - -Location::Location(const Angle &latitude, const Angle &lon) : - m_lat(latitude), - m_lon(lon), - m_ele(0.0) -{} - -Location::Location(const string &lat, const string &lon, Angle::AngularMeasure measure) : - m_lat(Angle(lat, measure)), - m_lon(Angle(lon, measure)), - m_ele(0.0) -{} - -Location::Location(const string &latitudeAndLongitude, Angle::AngularMeasure measure) : - m_ele(0.0) -{ - string::size_type dpos = latitudeAndLongitude.find(','); - if(dpos == string::npos) - throw Failure("Pair of coordinates (latitude and longitude) required."); - else if(dpos >= (latitudeAndLongitude.length() - 1)) - throw Failure("No second longitude following after comma."); - else if(latitudeAndLongitude.find(',', dpos + 1) != string::npos) - throw Failure("More then 2 coordinates given."); - m_lat = Angle(latitudeAndLongitude.substr(0, dpos), measure); - m_lon = Angle(latitudeAndLongitude.substr(dpos + 1), measure); -} - -Location::~Location() -{} - -string Location::toString(Angle::OutputForm form) const -{ - return m_lat.toString(form) + "," + m_lon.toString(form); -} - -string Location::toUtmWgs4String() const -{ - int zone; - char zoneDesignator; - double east, north; - computeUtmWgs4Coordinates(zone, zoneDesignator, east, north); - stringstream ss(stringstream::in | stringstream::out); - ss << setprecision(0) << fixed; - ss << zone << zoneDesignator << "E" << east << "N" << north; - return ss.str(); -} - -double Location::distanceTo(const Location &location) const -{ - double lat1 = m_lat.radianValue(); - double lon1 = m_lon.radianValue(); - double lat2 = location.m_lat.radianValue(); - double lon2 = location.m_lon.radianValue(); - double latd = lat1 - lat2; - double lond = lon1 - lon2; - latd = sin(latd / 2.0); - lond = sin(lond / 2.0); - double a = latd * latd + lond * lond * cos(lat1) * cos(lat2); - return m_er * 2.0 * atan2(sqrt(a), sqrt(1.0 - a)); -} - -Angle Location::initialBearingTo(const Location &location) const -{ - double lat1 = m_lat.radianValue(); - double lon1 = m_lon.radianValue(); - double lat2 = location.m_lat.radianValue(); - double lon2 = location.m_lon.radianValue(); - double lond = lon2 - lon1; - double b = atan2(sin(lond) * cos(lat2), cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lond)); - Angle angle(b); - angle.adjust0To360(); - return angle; -} - -Angle Location::finalBearingTo(const Location &location) const -{ - Angle angle(location.initialBearingTo(*this)); - angle.reverse(); - return angle; -} - -Location Location::destination(double distance, const Angle &bearing) -{ - double lat1 = m_lat.radianValue(); - double lon1 = m_lon.radianValue(); - double brng = bearing.radianValue(); - double ad = angularDistance(distance).radianValue(); - double lat2 = asin(sin(lat1) * cos(ad) + cos(lat1) * sin(ad) * cos(brng)); - double lon2 = lon1 + atan2(sin(brng) * sin(ad) * cos(lat1), cos(ad) - sin(lat1) * sin(lat2)); - return Location(Angle(lat2), Angle(lon2)); -} - -void Location::computeUtmWgs4Coordinates(int &zone, char &zoneDesignator, double &east, double &north) const -{ - double a = WGS84_A; - double eccSquared = UTM_E2; - double k0 = UTM_K0; - - double latd = m_lat.degreeValue(); - double lond = m_lon.degreeValue(); - double latr = m_lat.radianValue(); - double lonr = m_lon.radianValue(); - - zone = int((lond + 180) / 6) + 1; - - if(latd >= 56.0 && latd < 64.0 && lond >= 3.0 && lond < 12.0) - zone = 32; - - // Special zones for Svalbard - if(latd >= 72.0 && latd < 84.0) - { - if(lond >= 0.0 && lond < 9.0) zone = 31; - else if(lond >= 9.0 && lond < 21.0) zone = 33; - else if(lond >= 21.0 && lond < 33.0) zone = 35; - else if(lond >= 33.0 && lond < 42.0) zone = 37; - } - - zoneDesignator = computeUtmZoneDesignator(); - - // +3 puts origin in middle of zone - double lonOriginr = Angle((zone - 1) * 6 - 180 + 3, Angle::AngularMeasure::Degree).radianValue(); - - double eccPrimeSquared = (eccSquared) / (1 - eccSquared); - double N = a / sqrt(1 - eccSquared * sin(latr) * sin(latr)); - double T = tan(latr) * tan(latr); - double C = eccPrimeSquared * cos(latr) * cos(latr); - double A = cos(latr) * (lonr-lonOriginr); - double M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - - 5 * eccSquared * eccSquared * eccSquared / 256) * latr - - (3 * eccSquared / 8 + 3 * eccSquared*eccSquared / 32 - + 45 * eccSquared * eccSquared * eccSquared / 1024)*sin(2 * latr) - + (15 * eccSquared * eccSquared / 256 - + 45 * eccSquared * eccSquared*eccSquared / 1024) * sin(4 * latr) - - (35 * eccSquared * eccSquared * eccSquared/3072) * sin(6 * latr)); - - east = static_cast - (k0 * N * (A + (1 - T + C) * A * A * A / 6 - + (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120) - + 500000.0); - - north = static_cast - (k0 * (M + N * tan(latr) - * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A *A / 24 - + (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720))); - - if(latd < 0) - north += 10000000.0; -} - -Location Location::midpoint(const Location &location1, const Location &location2) -{ - double lat1 = location1.m_lat.radianValue(); - double lon1 = location1.m_lon.radianValue(); - double lat2 = location2.m_lat.radianValue(); - double lon2 = location2.m_lon.radianValue(); - double lond = lon2 - lon1; - double x = cos(lat2) * cos(lond); - double y = cos(lat2) * sin(lond); - return Location( - Angle(atan2(sin(lat1) + sin(lat2), sqrt((cos(lat1) + x) * (cos(lat1) + x) + y * y))), - Angle(lon1 + atan2(y, cos(lat1) + x))); -} - -double Location::trackLength(const std::vector &track, bool circle) -{ - if(track.size() < 2) - throw Failure("At least two locations are required to calculate a distance."); - - const Location *location1 = &track.at(0); - const Location *location2 = &track.at(1); - double distance = location1->distanceTo(*location2); - - for(std::vector::const_iterator i = track.cbegin() + 2, end = track.cend(); i != end; ++i) { - location1 = location2; - location2 = &(*i); - distance += location1->distanceTo(*location2); - } - - if(circle) - distance += track.front().distanceTo(track.back()); - - return distance; -} - -double Location::earthRadius() -{ - return m_er; -} - -Angle Location::angularDistance(double distance) -{ - return Angle(distance / m_er); -} - -char Location::computeUtmZoneDesignator() const -{ - double l = m_lat.degreeValue(); - if ((84 >= l) && (l >= 72)) return 'X'; - else if ((72 > l) && (l >= 64)) return 'W'; - else if ((64 > l) && (l >= 56)) return 'V'; - else if ((56 > l) && (l >= 48)) return 'U'; - else if ((48 > l) && (l >= 40)) return 'T'; - else if ((40 > l) && (l >= 32)) return 'S'; - else if ((32 > l) && (l >= 24)) return 'R'; - else if ((24 > l) && (l >= 16)) return 'Q'; - else if ((16 > l) && (l >= 8)) return 'P'; - else if (( 8 > l) && (l >= 0)) return 'N'; - else if (( 0 > l) && (l >= -8)) return 'M'; - else if ((-8 > l) && (l >= -16)) return 'L'; - else if((-16 > l) && (l >= -24)) return 'K'; - else if((-24 > l) && (l >= -32)) return 'J'; - else if((-32 > l) && (l >= -40)) return 'H'; - else if((-40 > l) && (l >= -48)) return 'G'; - else if((-48 > l) && (l >= -56)) return 'F'; - else if((-56 > l) && (l >= -64)) return 'E'; - else if((-64 > l) && (l >= -72)) return 'D'; - else if((-72 > l) && (l >= -80)) return 'C'; - else return '\0'; -} - -void Location::setValueByProvidedUtmWgs4Coordinates(const string &utmWgs4Coordinates) -{ - string::size_type epos = utmWgs4Coordinates.find('E'); - if(epos != 0 && epos != string::npos) { - string::size_type npos = utmWgs4Coordinates.find('N', epos); - if(npos < (utmWgs4Coordinates.length() - 1) && npos != string::npos) { - int zone = ConversionUtilities::numberFromString(utmWgs4Coordinates.substr(0, epos - 1)); - char zoneDesignator = utmWgs4Coordinates.at(epos - 1); - double east = ConversionUtilities::numberFromString(utmWgs4Coordinates.substr(epos + 1, npos - epos - 1)); - double north = ConversionUtilities::numberFromString(utmWgs4Coordinates.substr(npos + 1)); - setValueByProvidedUtmWgs4Coordinates(zone, zoneDesignator, east, north); - return; - } - } - throw Failure("UTM coordinates incomplete."); -} - -void Location::setValueByProvidedUtmWgs4Coordinates(int zone, char zoneDesignator, double easting, double northing) -{ - double k0 = UTM_K0; - double a = WGS84_A; - double eccSquared = UTM_E2; - double eccPrimeSquared; - double e1 = (1 - sqrt(1 - eccSquared)) / (1 + sqrt(1 - eccSquared)); - - double x = easting - 500000.0; //remove 500,000 meter offset for longitude - double y = northing; - - if((zoneDesignator - 'N') < 0) - //remove 10,000,000 meter offset used for southern hemisphere - y -= 10000000.0; - - //+3 puts origin in middle of zone - double longOriginr = Angle((zone - 1) * 6 - 180 + 3, Angle::AngularMeasure::Degree).radianValue(); - eccPrimeSquared = (eccSquared)/(1-eccSquared); - - double M = y / k0; - double mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - -5 * eccSquared * eccSquared * eccSquared / 256)); - - double phi1Rad = mu + ((3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * mu) - + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * mu) - + (151 * e1 * e1 * e1 / 96) * sin(6 * mu)); - - double N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad)); - double T1 = tan(phi1Rad) * tan(phi1Rad); - double C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad); - double R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5); - double D = x / (N1 * k0); - - m_lat = Angle(phi1Rad - ((N1 * tan(phi1Rad) / R1) - * (D * D / 2 - -(5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * eccPrimeSquared) * D * D * D * D / 24 - +(61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - -3 * C1 * C1) * D * D * D * D * D * D / 720))); - m_lat.adjust180To180(); - m_lon = Angle(((D - (1 + 2 * T1 + C1) * D * D * D / 6 - +(5 - 2 * C1 + 28 * T1 - 3 * C1 * C1 + 8 * eccPrimeSquared + 24 * T1 * T1) - * D * D * D * D * D / 120) - / cos(phi1Rad)) + longOriginr); - m_lon.adjust180To180(); -} - -const double Location::m_er = 6371000.0; +#include "./location.h" +#include "./utils.h" + +#include + +#include +#include +#include + +using namespace std; +using namespace ApplicationUtilities; + +// WGS84 Parameters +#define WGS84_A 6378137.0 // major axis +#define WGS84_B 6356752.31424518 // minor axis +#define WGS84_F 0.0033528107 // ellipsoid flattening +#define WGS84_E 0.0818191908 // first eccentricity +#define WGS84_EP 0.0820944379 // second eccentricity + +// UTM Parameters +#define UTM_K0 0.9996 // scale factor +#define UTM_FE 500000.0 // false easting +#define UTM_FN_N 0.0 // false northing, northern hemisphere +#define UTM_FN_S 10000000.0 // false northing, southern hemisphere +#define UTM_E2 (WGS84_E * WGS84_E) // e^2 +#define UTM_E4 (UTM_E2 * UTM_E2) // e^4 +#define UTM_E6 (UTM_E4 * UTM_E2) // e^6 +#define UTM_EP2 (UTM_E2 / (1 - UTM_E2)) // e'^2 + +Location::Location() : + m_lat(0.0), + m_lon(0.0), + m_ele(0.0) +{} + +Location::Location(const Angle &latitude, const Angle &lon) : + m_lat(latitude), + m_lon(lon), + m_ele(0.0) +{} + +Location::Location(const string &lat, const string &lon, Angle::AngularMeasure measure) : + m_lat(Angle(lat, measure)), + m_lon(Angle(lon, measure)), + m_ele(0.0) +{} + +Location::Location(const string &latitudeAndLongitude, Angle::AngularMeasure measure) : + m_ele(0.0) +{ + string::size_type dpos = latitudeAndLongitude.find(','); + if(dpos == string::npos) + throw Failure("Pair of coordinates (latitude and longitude) required."); + else if(dpos >= (latitudeAndLongitude.length() - 1)) + throw Failure("No second longitude following after comma."); + else if(latitudeAndLongitude.find(',', dpos + 1) != string::npos) + throw Failure("More then 2 coordinates given."); + m_lat = Angle(latitudeAndLongitude.substr(0, dpos), measure); + m_lon = Angle(latitudeAndLongitude.substr(dpos + 1), measure); +} + +Location::~Location() +{} + +string Location::toString(Angle::OutputForm form) const +{ + return m_lat.toString(form) + "," + m_lon.toString(form); +} + +string Location::toUtmWgs4String() const +{ + int zone; + char zoneDesignator; + double east, north; + computeUtmWgs4Coordinates(zone, zoneDesignator, east, north); + stringstream ss(stringstream::in | stringstream::out); + ss << setprecision(0) << fixed; + ss << zone << zoneDesignator << "E" << east << "N" << north; + return ss.str(); +} + +double Location::distanceTo(const Location &location) const +{ + double lat1 = m_lat.radianValue(); + double lon1 = m_lon.radianValue(); + double lat2 = location.m_lat.radianValue(); + double lon2 = location.m_lon.radianValue(); + double latd = lat1 - lat2; + double lond = lon1 - lon2; + latd = sin(latd / 2.0); + lond = sin(lond / 2.0); + double a = latd * latd + lond * lond * cos(lat1) * cos(lat2); + return m_er * 2.0 * atan2(sqrt(a), sqrt(1.0 - a)); +} + +Angle Location::initialBearingTo(const Location &location) const +{ + double lat1 = m_lat.radianValue(); + double lon1 = m_lon.radianValue(); + double lat2 = location.m_lat.radianValue(); + double lon2 = location.m_lon.radianValue(); + double lond = lon2 - lon1; + double b = atan2(sin(lond) * cos(lat2), cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lond)); + Angle angle(b); + angle.adjust0To360(); + return angle; +} + +Angle Location::finalBearingTo(const Location &location) const +{ + Angle angle(location.initialBearingTo(*this)); + angle.reverse(); + return angle; +} + +Location Location::destination(double distance, const Angle &bearing) +{ + double lat1 = m_lat.radianValue(); + double lon1 = m_lon.radianValue(); + double brng = bearing.radianValue(); + double ad = angularDistance(distance).radianValue(); + double lat2 = asin(sin(lat1) * cos(ad) + cos(lat1) * sin(ad) * cos(brng)); + double lon2 = lon1 + atan2(sin(brng) * sin(ad) * cos(lat1), cos(ad) - sin(lat1) * sin(lat2)); + return Location(Angle(lat2), Angle(lon2)); +} + +void Location::computeUtmWgs4Coordinates(int &zone, char &zoneDesignator, double &east, double &north) const +{ + double a = WGS84_A; + double eccSquared = UTM_E2; + double k0 = UTM_K0; + + double latd = m_lat.degreeValue(); + double lond = m_lon.degreeValue(); + double latr = m_lat.radianValue(); + double lonr = m_lon.radianValue(); + + zone = int((lond + 180) / 6) + 1; + + if(latd >= 56.0 && latd < 64.0 && lond >= 3.0 && lond < 12.0) + zone = 32; + + // Special zones for Svalbard + if(latd >= 72.0 && latd < 84.0) + { + if(lond >= 0.0 && lond < 9.0) zone = 31; + else if(lond >= 9.0 && lond < 21.0) zone = 33; + else if(lond >= 21.0 && lond < 33.0) zone = 35; + else if(lond >= 33.0 && lond < 42.0) zone = 37; + } + + zoneDesignator = computeUtmZoneDesignator(); + + // +3 puts origin in middle of zone + double lonOriginr = Angle((zone - 1) * 6 - 180 + 3, Angle::AngularMeasure::Degree).radianValue(); + + double eccPrimeSquared = (eccSquared) / (1 - eccSquared); + double N = a / sqrt(1 - eccSquared * sin(latr) * sin(latr)); + double T = tan(latr) * tan(latr); + double C = eccPrimeSquared * cos(latr) * cos(latr); + double A = cos(latr) * (lonr-lonOriginr); + double M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 + - 5 * eccSquared * eccSquared * eccSquared / 256) * latr + - (3 * eccSquared / 8 + 3 * eccSquared*eccSquared / 32 + + 45 * eccSquared * eccSquared * eccSquared / 1024)*sin(2 * latr) + + (15 * eccSquared * eccSquared / 256 + + 45 * eccSquared * eccSquared*eccSquared / 1024) * sin(4 * latr) + - (35 * eccSquared * eccSquared * eccSquared/3072) * sin(6 * latr)); + + east = static_cast + (k0 * N * (A + (1 - T + C) * A * A * A / 6 + + (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120) + + 500000.0); + + north = static_cast + (k0 * (M + N * tan(latr) + * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A *A / 24 + + (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720))); + + if(latd < 0) + north += 10000000.0; +} + +Location Location::midpoint(const Location &location1, const Location &location2) +{ + double lat1 = location1.m_lat.radianValue(); + double lon1 = location1.m_lon.radianValue(); + double lat2 = location2.m_lat.radianValue(); + double lon2 = location2.m_lon.radianValue(); + double lond = lon2 - lon1; + double x = cos(lat2) * cos(lond); + double y = cos(lat2) * sin(lond); + return Location( + Angle(atan2(sin(lat1) + sin(lat2), sqrt((cos(lat1) + x) * (cos(lat1) + x) + y * y))), + Angle(lon1 + atan2(y, cos(lat1) + x))); +} + +double Location::trackLength(const std::vector &track, bool circle) +{ + if(track.size() < 2) + throw Failure("At least two locations are required to calculate a distance."); + + const Location *location1 = &track.at(0); + const Location *location2 = &track.at(1); + double distance = location1->distanceTo(*location2); + + for(std::vector::const_iterator i = track.cbegin() + 2, end = track.cend(); i != end; ++i) { + location1 = location2; + location2 = &(*i); + distance += location1->distanceTo(*location2); + } + + if(circle) + distance += track.front().distanceTo(track.back()); + + return distance; +} + +double Location::earthRadius() +{ + return m_er; +} + +Angle Location::angularDistance(double distance) +{ + return Angle(distance / m_er); +} + +char Location::computeUtmZoneDesignator() const +{ + double l = m_lat.degreeValue(); + if ((84 >= l) && (l >= 72)) return 'X'; + else if ((72 > l) && (l >= 64)) return 'W'; + else if ((64 > l) && (l >= 56)) return 'V'; + else if ((56 > l) && (l >= 48)) return 'U'; + else if ((48 > l) && (l >= 40)) return 'T'; + else if ((40 > l) && (l >= 32)) return 'S'; + else if ((32 > l) && (l >= 24)) return 'R'; + else if ((24 > l) && (l >= 16)) return 'Q'; + else if ((16 > l) && (l >= 8)) return 'P'; + else if (( 8 > l) && (l >= 0)) return 'N'; + else if (( 0 > l) && (l >= -8)) return 'M'; + else if ((-8 > l) && (l >= -16)) return 'L'; + else if((-16 > l) && (l >= -24)) return 'K'; + else if((-24 > l) && (l >= -32)) return 'J'; + else if((-32 > l) && (l >= -40)) return 'H'; + else if((-40 > l) && (l >= -48)) return 'G'; + else if((-48 > l) && (l >= -56)) return 'F'; + else if((-56 > l) && (l >= -64)) return 'E'; + else if((-64 > l) && (l >= -72)) return 'D'; + else if((-72 > l) && (l >= -80)) return 'C'; + else return '\0'; +} + +void Location::setValueByProvidedUtmWgs4Coordinates(const string &utmWgs4Coordinates) +{ + string::size_type epos = utmWgs4Coordinates.find('E'); + if(epos != 0 && epos != string::npos) { + string::size_type npos = utmWgs4Coordinates.find('N', epos); + if(npos < (utmWgs4Coordinates.length() - 1) && npos != string::npos) { + int zone = ConversionUtilities::numberFromString(utmWgs4Coordinates.substr(0, epos - 1)); + char zoneDesignator = utmWgs4Coordinates.at(epos - 1); + double east = ConversionUtilities::numberFromString(utmWgs4Coordinates.substr(epos + 1, npos - epos - 1)); + double north = ConversionUtilities::numberFromString(utmWgs4Coordinates.substr(npos + 1)); + setValueByProvidedUtmWgs4Coordinates(zone, zoneDesignator, east, north); + return; + } + } + throw Failure("UTM coordinates incomplete."); +} + +void Location::setValueByProvidedUtmWgs4Coordinates(int zone, char zoneDesignator, double easting, double northing) +{ + double k0 = UTM_K0; + double a = WGS84_A; + double eccSquared = UTM_E2; + double eccPrimeSquared; + double e1 = (1 - sqrt(1 - eccSquared)) / (1 + sqrt(1 - eccSquared)); + + double x = easting - 500000.0; //remove 500,000 meter offset for longitude + double y = northing; + + if((zoneDesignator - 'N') < 0) + //remove 10,000,000 meter offset used for southern hemisphere + y -= 10000000.0; + + //+3 puts origin in middle of zone + double longOriginr = Angle((zone - 1) * 6 - 180 + 3, Angle::AngularMeasure::Degree).radianValue(); + eccPrimeSquared = (eccSquared)/(1-eccSquared); + + double M = y / k0; + double mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 + -5 * eccSquared * eccSquared * eccSquared / 256)); + + double phi1Rad = mu + ((3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * mu) + + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * mu) + + (151 * e1 * e1 * e1 / 96) * sin(6 * mu)); + + double N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad)); + double T1 = tan(phi1Rad) * tan(phi1Rad); + double C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad); + double R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5); + double D = x / (N1 * k0); + + m_lat = Angle(phi1Rad - ((N1 * tan(phi1Rad) / R1) + * (D * D / 2 + -(5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * eccPrimeSquared) * D * D * D * D / 24 + +(61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared + -3 * C1 * C1) * D * D * D * D * D * D / 720))); + m_lat.adjust180To180(); + m_lon = Angle(((D - (1 + 2 * T1 + C1) * D * D * D / 6 + +(5 - 2 * C1 + 28 * T1 - 3 * C1 * C1 + 8 * eccPrimeSquared + 24 * T1 * T1) + * D * D * D * D * D / 120) + / cos(phi1Rad)) + longOriginr); + m_lon.adjust180To180(); +} + +const double Location::m_er = 6371000.0; diff --git a/location.h b/location.h index cdbe935..d19e2ab 100644 --- a/location.h +++ b/location.h @@ -1,88 +1,88 @@ -#ifndef LOCATION_H -#define LOCATION_H - -#include "./angle.h" - -#include -#include - -class Location -{ -public: - enum class GeographicCoordinateSystem - { - LatitudeAndLongitude, - UTMWGS84 - }; - - Location(); - Location(const Angle &lat, const Angle &lon); - explicit Location(const std::string &lat, const std::string &lon, Angle::AngularMeasure measure = Angle::AngularMeasure::Radian); - explicit Location(const std::string &latitudeAndLongitude, Angle::AngularMeasure measure = Angle::AngularMeasure::Radian); - ~Location(); - - const Angle &latitude() const; - void setLatitude(const Angle &value); - const Angle &longitude() const; - void setLongitude(const Angle &value); - double elevation() const; - void setElevation(double value); - std::string toString(Angle::OutputForm form = Angle::OutputForm::Degrees) const; - std::string toUtmWgs4String() const; - bool isEmpty() const; - double distanceTo(const Location &location) const; - Angle initialBearingTo(const Location &location) const; - Angle finalBearingTo(const Location &location) const; - Location destination(double distance, const Angle &bearing); - void computeUtmWgs4Coordinates(int &zone, char &zoneDesignator, double &east, double &north) const; - char computeUtmZoneDesignator() const; - void setValueByProvidedUtmWgs4Coordinates(const std::string &utmWgs4Coordinates); - void setValueByProvidedUtmWgs4Coordinates(int zone, char zoneDesignator, double east, double north); - static Location midpoint(const Location &location1, const Location &location2); - static double trackLength(const std::vector &track, bool circle = false); - static double earthRadius(); - static Angle angularDistance(double distance); -protected: -private: - Angle m_lat; - Angle m_lon; - double m_ele; - static const double m_er; -}; - -inline const Angle &Location::latitude() const -{ - return m_lat; -} - -inline const Angle &Location::longitude() const -{ - return m_lon; -} - -inline double Location::elevation() const -{ - return m_ele; -} - -inline void Location::setLatitude(const Angle &value) -{ - m_lat = value; -} - -inline void Location::setLongitude(const Angle &value) -{ - m_lon = value; -} - -inline void Location::setElevation(double value) -{ - m_ele = value; -} - -inline bool Location::isEmpty() const -{ - return m_lat.isNull() && m_lon.isNull() && m_ele == 0.0; -} - -#endif // LOCATION_H +#ifndef LOCATION_H +#define LOCATION_H + +#include "./angle.h" + +#include +#include + +class Location +{ +public: + enum class GeographicCoordinateSystem + { + LatitudeAndLongitude, + UTMWGS84 + }; + + Location(); + Location(const Angle &lat, const Angle &lon); + explicit Location(const std::string &lat, const std::string &lon, Angle::AngularMeasure measure = Angle::AngularMeasure::Radian); + explicit Location(const std::string &latitudeAndLongitude, Angle::AngularMeasure measure = Angle::AngularMeasure::Radian); + ~Location(); + + const Angle &latitude() const; + void setLatitude(const Angle &value); + const Angle &longitude() const; + void setLongitude(const Angle &value); + double elevation() const; + void setElevation(double value); + std::string toString(Angle::OutputForm form = Angle::OutputForm::Degrees) const; + std::string toUtmWgs4String() const; + bool isEmpty() const; + double distanceTo(const Location &location) const; + Angle initialBearingTo(const Location &location) const; + Angle finalBearingTo(const Location &location) const; + Location destination(double distance, const Angle &bearing); + void computeUtmWgs4Coordinates(int &zone, char &zoneDesignator, double &east, double &north) const; + char computeUtmZoneDesignator() const; + void setValueByProvidedUtmWgs4Coordinates(const std::string &utmWgs4Coordinates); + void setValueByProvidedUtmWgs4Coordinates(int zone, char zoneDesignator, double east, double north); + static Location midpoint(const Location &location1, const Location &location2); + static double trackLength(const std::vector &track, bool circle = false); + static double earthRadius(); + static Angle angularDistance(double distance); +protected: +private: + Angle m_lat; + Angle m_lon; + double m_ele; + static const double m_er; +}; + +inline const Angle &Location::latitude() const +{ + return m_lat; +} + +inline const Angle &Location::longitude() const +{ + return m_lon; +} + +inline double Location::elevation() const +{ + return m_ele; +} + +inline void Location::setLatitude(const Angle &value) +{ + m_lat = value; +} + +inline void Location::setLongitude(const Angle &value) +{ + m_lon = value; +} + +inline void Location::setElevation(double value) +{ + m_ele = value; +} + +inline bool Location::isEmpty() const +{ + return m_lat.isNull() && m_lon.isNull() && m_ele == 0.0; +} + +#endif // LOCATION_H