VSM C++ SDK
Vehicle Specific Modules SDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
coordinates.h
Go to the documentation of this file.
1 // Copyright (c) 2018, Smart Projects Holdings Ltd
2 // All rights reserved.
3 // See LICENSE file for license details.
4 
11 #ifndef _UGCS_VSM_COORDINATES_H_
12 #define _UGCS_VSM_COORDINATES_H_
13 
14 #include <ugcs/vsm/exception.h>
15 #include <ugcs/vsm/math.h>
16 
17 #include <cmath>
18 
19 namespace ugcs {
20 namespace vsm {
21 
23 class Wgs84_datum {
24 public:
26  static constexpr double FLATTENING = 1.0 / 298.257223563;
27 
29  static constexpr double EQUATORIAL_RADIUS = 6378137.0;
30 
32  static constexpr double POLAR_RADIUS = 6356752.3;
33 };
34 
37 public:
39  double latitude,
41  longitude,
43  altitude;
44 
46  Geodetic_tuple(double latitude, double longitude, double altitude):
47  latitude(latitude), longitude(longitude), altitude(altitude)
48  {}
49 };
50 
53 public:
54  // @{
56  double x, y, z;
57  // @}
58 
60  Cartesian_tuple(double x, double y, double z):
61  x(x), y(y), z(z)
62  {}
63 };
64 
66 template <class Datum>
67 class Position {
68 public:
70  typedef Datum Datum_type;
71 
73  static constexpr double ECCENTRICITY_SQUARED = (2 - Datum::FLATTENING) * Datum::FLATTENING;
74 
77  coord(Normalize_latitude(tuple.latitude),
78  Normalize_longitude(tuple.longitude),
79  tuple.altitude),
80  ecef_coord(Calculate_ecef(coord))
81  {}
82 
85  coord(From_ecef(tuple)), ecef_coord(tuple)
86  {}
87 
90  Get_geodetic() const
91  {
92  return coord;
93  }
94 
102  Get_ecef() const
103  {
104  return ecef_coord;
105  }
106 
108  double
109  Lat_meter() const
110  {
111  // XXX valid only for WGS-84
112  static constexpr double M1 = 111132.92;
113  static constexpr double M2 = -559.82;
114  static constexpr double M3 = 1.175;
115  static constexpr double M4 = -0.0023;
116  double latlen = M1 +
117  (M2 * cos(2.0 * coord.latitude)) +
118  (M3 * cos(4.0 * coord.latitude)) +
119  (M4 * cos(6.0 * coord.latitude));
120  return M_PI / latlen / 180.0;
121  }
122 
124  double
125  Long_meter() const
126  {
127  // XXX valid only for WGS-84
128  static constexpr double P1 = 111412.84;
129  static constexpr double P2 = -93.5;
130  static constexpr double P3 = 0.118;
131  double longlen = P1 * cos(coord.latitude) +
132  P2 * cos(3.0 * coord.latitude) +
133  P3 * cos(5.0 * coord.latitude);
134  return M_PI / longlen / 180.0;
135  }
136 
138  double
139  Bearing(const Position &target)
140  {
141  return atan2(
142  cos(target.coord.latitude) * sin(target.coord.longitude - coord.longitude),
143  cos(coord.latitude) * sin(target.coord.latitude) -
144  sin(coord.latitude) * cos(target.coord.latitude) * cos(target.coord.longitude - coord.longitude));
145  }
146 
150  double
151  Earth_radius() const
152  {
153  static constexpr double n =
154  Datum::EQUATORIAL_RADIUS * Datum::EQUATORIAL_RADIUS *
155  Datum::POLAR_RADIUS;
156  double d1 = Datum::EQUATORIAL_RADIUS * std::cos(Get_geodetic().latitude);
157  double d2 = Datum::POLAR_RADIUS * std::sin(Get_geodetic().latitude);
158  return n / (d1 * d1 + d2 * d2);
159  }
160 
165  double
166  Distance(const Position& pos) const
167  {
168  auto p1 = Get_geodetic();
169  auto p2 = pos.Get_geodetic();
170  Position avg(Geodetic_tuple((p1.latitude + p2.latitude) / 2, 0, 0));
171  double acos_arg = std::sin(p1.latitude) * std::sin(p2.latitude) +
172  std::cos(p1.latitude) * std::cos(p2.latitude) *
173  std::cos(p2.longitude - p1.longitude);
174  /* It was noticed (at least on Windows builds), that the formula above
175  * might give values which are slightly more than 1 (or less than -1)
176  * due to calculation errors. This happens when coordinates are very
177  * close. Normalize it to be in the range [-1; 1] to avoid NaN result
178  * from acos.
179  */
180  if (acos_arg > 1) {
181  acos_arg = 1;
182  } else if (acos_arg < -1) {
183  acos_arg = -1;
184  }
185  return std::acos(acos_arg) * avg.Earth_radius();
186  }
187 
188 private:
190  Geodetic_tuple coord;
191  Cartesian_tuple ecef_coord;
192 
199  static double
200  Normalize(double value, double base)
201  {
202  if (base <= 0) {
203  VSM_EXCEPTION(Exception, "Invalid base specified");
204  }
205  return value - base * std::floor(value / base);
206  }
207 
209  static double
210  Normalize_latitude(double value)
211  {
212  return Normalize(value + M_PI / 2, M_PI) - M_PI / 2;
213  }
214 
216  static double
217  Normalize_longitude(double value)
218  {
219  return Normalize(value + M_PI, 2 * M_PI) - M_PI;
220  }
221 
222  static double
223  Signum(double value)
224  {
225  return (value == 0.0 || std::isnan(value)) ? value : std::copysign(1.0, value);
226  }
227 
229  static Geodetic_tuple
230  From_ecef(const Cartesian_tuple &tuple)
231  {
232  double latitude, longitude, altitude;
233  double distance = std::hypot(tuple.x, tuple.y);
234 
235  if (distance == 0.0) {
236  latitude = 0.5 * M_PI * Signum(tuple.z);
237  longitude = 0.0;
238  double sine_latitude = std::sin(latitude);
239  altitude = tuple.z * sine_latitude - Datum::EQUATORIAL_RADIUS *
240  std::sqrt(1 - ECCENTRICITY_SQUARED * sine_latitude * sine_latitude);
241  } else {
242  double raw_longitude = std::abs(std::asin(tuple.y / distance));
243  if (tuple.y == 0.0) {
244  longitude = tuple.x > 0.0 ? 0 : M_PI;
245  } else {
246  if (tuple.x > 0.0) {
247  longitude = tuple.y > 0.0 ? raw_longitude : 2.0 * M_PI - raw_longitude;
248  } else {
249  longitude = tuple.y > 0.0 ? M_PI - raw_longitude : M_PI + raw_longitude;
250  }
251  }
252  if (tuple.z == 0.0) {
253  latitude = 0.0;
254  altitude = distance - Datum::EQUATORIAL_RADIUS;
255  } else {
256  /* Threshold for iterative process set at 0.00001 arc second. */
257  constexpr double threshold = 4.8481368110953605e-11;
258 
259  double radius = std::hypot(distance, tuple.z);
260  double inclination = std::asin(tuple.z / radius);
261  double ratio = ECCENTRICITY_SQUARED * Datum::EQUATORIAL_RADIUS / (2.0 * radius);
262 
263  /* Performing iterations. */
264  double latitude_estimate, correction, next_correction = 0.0,
265  sine, root;
266  int iterations = 0;
267  constexpr static int max_iterations = 1000;
268  do {
269  if (iterations > max_iterations) {
270  VSM_EXCEPTION(Exception, "Cannot find fixed point of the transform.");
271  }
272  iterations++;
273  correction = next_correction;
274  latitude_estimate = inclination + correction;
275  sine = std::sin(latitude_estimate);
276  root = std::sqrt(1.0 - ECCENTRICITY_SQUARED * sine * sine);
277  next_correction =
278  std::asin(ratio * std::sin(2.0 * latitude_estimate) / root);
279  } while (std::abs(next_correction - correction) >= threshold);
280 
281  latitude = latitude_estimate;
282  altitude = distance * std::cos(latitude) + tuple.z * sine -
283  Datum::EQUATORIAL_RADIUS * root;
284  }
285  }
286  return Geodetic_tuple(latitude, longitude, altitude);
287  }
288 
289  static Cartesian_tuple
290  Calculate_ecef(const Geodetic_tuple &coord)
291  {
292  double cosine_latitude = std::cos(coord.latitude);
293  double sine_latitude = std::sin(coord.latitude);
294  double cosine_longitude = std::cos(coord.longitude);
295  double sine_longitude = std::sin(coord.longitude);
296 
297  double curvature_radius = Datum::EQUATORIAL_RADIUS /
298  std::sqrt(1.0 - ECCENTRICITY_SQUARED * sine_latitude * sine_latitude);
299 
300  return Cartesian_tuple(
301  (curvature_radius + coord.altitude) * cosine_latitude * cosine_longitude,
302  (curvature_radius + coord.altitude) * cosine_latitude * sine_longitude,
303  ((1.0 - ECCENTRICITY_SQUARED) * curvature_radius + coord.altitude) * sine_latitude);
304  }
305 };
306 
309 
310 float
311 Normalize_angle_0_2pi(float a);
312 
313 float
314 Normalize_angle_minuspi_pi(float a);
315 
316 } /* namespace vsm */
317 } /* namespace ugcs */
318 
319 #endif /* _UGCS_VSM_COORDINATES_H_ */
Position(Geodetic_tuple tuple)
Construct position from geodetic coordinates.
Definition: coordinates.h:76
double Distance(const Position &pos) const
Calculate the surface (altitude is not taken into account) distance in meters between this and given ...
Definition: coordinates.h:166
static constexpr double FLATTENING
Flattening of the reference ellipsoid.
Definition: coordinates.h:26
Position< Wgs84_datum > Wgs84_position
Position defined in WGS84 geodetic system.
Definition: coordinates.h:308
Coordinates tuple for geodetic CS.
Definition: coordinates.h:36
double x
In meters.
Definition: coordinates.h:56
static constexpr double EQUATORIAL_RADIUS
Equatorial radius of the reference ellipsoid, in meters.
Definition: coordinates.h:29
double Earth_radius() const
The Earth's mean radius of curvature (averaging over all directions) at a latitude of the position...
Definition: coordinates.h:151
VSM exceptions definition.
Geodetic_tuple(double latitude, double longitude, double altitude)
Construct the tuple with specific values.
Definition: coordinates.h:46
Datum for WGS84 geodetic system.
Definition: coordinates.h:23
Position(Cartesian_tuple tuple)
Construct position from ECEF coordinates.
Definition: coordinates.h:84
Datum Datum_type
Associated datum.
Definition: coordinates.h:70
static constexpr double POLAR_RADIUS
Polar radius of the reference ellipsoid, in meters.
Definition: coordinates.h:32
Coordinates tuple for cartesian CS.
Definition: coordinates.h:52
double latitude
Latitude value, radians.
Definition: coordinates.h:39
double Bearing(const Position &target)
Get bearing in radians to the target.
Definition: coordinates.h:139
Cartesian_tuple Get_ecef() const
Get representation in ECEF coordinates - Earth-centered Earth-fixed CS, ((x) axis points from the Ear...
Definition: coordinates.h:102
Immutable position in a specified coordinates system.
Definition: coordinates.h:67
double Long_meter() const
One meter expressed in longitude radians.
Definition: coordinates.h:125
double longitude
Longitude value, radians.
Definition: coordinates.h:39
double Lat_meter() const
One meter expressed in latitude radians.
Definition: coordinates.h:109
static constexpr double ECCENTRICITY_SQUARED
Square of eccentricity of the reference ellipsoid.
Definition: coordinates.h:73
Geodetic_tuple Get_geodetic() const
Get representation in geodetic coordinates.
Definition: coordinates.h:90
double altitude
Altitude value, meters.
Definition: coordinates.h:39
#define VSM_EXCEPTION(__exc_class, __msg,...)
Throw VSM exception instance.
Definition: exception.h:168
T value
Stored value (in wire byte order).
Definition: endian.h:356
Cartesian_tuple(double x, double y, double z)
Construct the tuple with specific values.
Definition: coordinates.h:60
Base class for all VSM exceptions.
Definition: exception.h:22