VSM C++ SDK
Vehicle Specific Modules SDK
coordinates.h
Go to the documentation of this file.
1 // Copyright (c) 2014, Smart Projects Holdings Ltd
2 // All rights reserved.
3 // See LICENSE file for license details.
4 
11 #ifndef COORDINATES_H_
12 #define COORDINATES_H_
13 
14 #include <ugcs/vsm/exception.h>
15 
16 #include <cmath>
17 
18 /* Might not be defined in <cmath> (not standard). */
19 #ifndef M_PI
20 
21 #define M_PI 3.14159265358979323846
22 #endif
23 
24 namespace ugcs {
25 namespace vsm {
26 
28 class Wgs84_datum {
29 public:
31  static constexpr double FLATTENING = 1.0 / 298.257223563;
32 
34  static constexpr double EQUATORIAL_RADIUS = 6378137.0;
35 
37  static constexpr double POLAR_RADIUS = 6356752.3;
38 };
39 
42 public:
44  double latitude,
46  longitude,
48  altitude;
49 
51  Geodetic_tuple(double latitude, double longitude, double altitude):
52  latitude(latitude), longitude(longitude), altitude(altitude)
53  {}
54 };
55 
58 public:
59  // @{
61  double x, y, z;
62  // @}
63 
65  Cartesian_tuple(double x, double y, double z):
66  x(x), y(y), z(z)
67  {}
68 };
69 
71 template <class Datum>
72 class Position {
73 public:
75  typedef Datum Datum_type;
76 
78  static constexpr double ECCENTRICITY_SQUARED = (2 - Datum::FLATTENING) * Datum::FLATTENING;
79 
82  coord(Normalize_latitude(tuple.latitude),
83  Normalize_longitude(tuple.longitude),
84  tuple.altitude),
85  ecef_coord(Calculate_ecef(coord))
86  {}
87 
90  coord(From_ecef(tuple)), ecef_coord(tuple)
91  {}
92 
95  Get_geodetic() const
96  {
97  return coord;
98  }
99 
107  Get_ecef() const
108  {
109  return ecef_coord;
110  }
111 
113  double
114  Lat_meter() const
115  {
116  //XXX valid only for WGS-84
117  static constexpr double M1 = 111132.92;
118  static constexpr double M2 = -559.82;
119  static constexpr double M3 = 1.175;
120  static constexpr double M4 = -0.0023;
121  double latlen = M1 +
122  (M2 * cos(2.0 * coord.latitude)) +
123  (M3 * cos(4.0 * coord.latitude)) +
124  (M4 * cos(6.0 * coord.latitude));
125  return M_PI / latlen / 180.0;
126  }
127 
129  double
130  Long_meter() const
131  {
132  //XXX valid only for WGS-84
133  static constexpr double P1 = 111412.84;
134  static constexpr double P2 = -93.5;
135  static constexpr double P3 = 0.118;
136  double longlen = P1 * cos(coord.latitude) +
137  P2 * cos(3.0 * coord.latitude) +
138  P3 * cos(5.0 * coord.latitude);
139  return M_PI / longlen / 180.0;
140  }
141 
143  double
144  Bearing(const Position &target)
145  {
146  double lat_m = (Lat_meter() + target.Lat_meter()) / 2.0;
147  double long_m = (Long_meter() + target.Long_meter()) / 2.0;
148  double d_lat = (target.coord.latitude - coord.latitude) / lat_m;
149  double d_long = (target.coord.longitude - coord.longitude) / long_m;
150  double d = sqrt(d_lat * d_lat + d_long * d_long);
151  double r = d_lat / d;
152  if (r > 1.0) {
153  r = 1.0;
154  } else if (r < -1.0) {
155  r = -1.0;
156  }
157  double a = std::acos(r);
158  if (d_long < 0) {
159  a = -a;
160  }
161  return a;
162  }
163 
167  double
168  Earth_radius() const
169  {
170  static constexpr double n =
171  Datum::EQUATORIAL_RADIUS * Datum::EQUATORIAL_RADIUS *
172  Datum::POLAR_RADIUS;
173  double d1 = Datum::EQUATORIAL_RADIUS * std::cos(Get_geodetic().latitude);
174  double d2 = Datum::POLAR_RADIUS * std::sin(Get_geodetic().latitude);
175  return n / (d1 * d1 + d2 * d2);
176  }
177 
182  double
183  Distance(const Position& pos) const
184  {
185  auto p1 = Get_geodetic();
186  auto p2 = pos.Get_geodetic();
187  Position avg(Geodetic_tuple((p1.latitude + p2.latitude) / 2, 0, 0));
188  double acos_arg = std::sin(p1.latitude) * std::sin(p2.latitude) +
189  std::cos(p1.latitude) * std::cos(p2.latitude) *
190  std::cos(p2.longitude - p1.longitude);
191  /* It was noticed (at least on Windows builds), that the formula above
192  * might give values which are slightly more than 1 (or less than -1)
193  * due to calculation errors. This happens when coordinates are very
194  * close. Normalize it to be in the range [-1; 1] to avoid NaN result
195  * from acos.
196  */
197  if (acos_arg > 1) {
198  acos_arg = 1;
199  } else if (acos_arg < -1) {
200  acos_arg = -1;
201  }
202  return std::acos(acos_arg) * avg.Earth_radius();
203  }
204 
205 private:
207  Geodetic_tuple coord;
208  Cartesian_tuple ecef_coord;
209 
216  static double
217  Normalize(double value, double base)
218  {
219  if (base <= 0) {
220  VSM_EXCEPTION(Exception, "Invalid base specified");
221  }
222  return value - base * std::floor(value / base);
223  }
224 
226  static double
227  Normalize_latitude(double value)
228  {
229  return Normalize(value + M_PI / 2, M_PI) - M_PI / 2;
230  }
231 
233  static double
234  Normalize_longitude(double value)
235  {
236  return Normalize(value + M_PI, 2 * M_PI) - M_PI;
237  }
238 
239  static double
240  Signum(double value)
241  {
242  return (value == 0.0 || std::isnan(value)) ? value : std::copysign(1.0, value);
243  }
244 
246  static Geodetic_tuple
247  From_ecef(const Cartesian_tuple &tuple)
248  {
249  /* Threshold for iterative process set at 0.00001 arc second. */
250  constexpr double threshold = 4.8481368110953605e-11;
251 
252  double latitude, longitude, altitude;
253  double distance = std::hypot(tuple.x, tuple.y);
254 
255  if (distance == 0.0) {
256  latitude = 0.5 * M_PI * Signum(tuple.z);
257  longitude = 0.0;
258  double sine_latitude = std::sin(latitude);
259  altitude = tuple.z * sine_latitude - Datum::EQUATORIAL_RADIUS *
260  std::sqrt(1 - ECCENTRICITY_SQUARED * sine_latitude * sine_latitude);
261  } else {
262  double raw_longitude = std::abs(std::asin(tuple.y / distance));
263  if (tuple.y == 0.0) {
264  longitude = tuple.x > 0.0 ? 0 : M_PI;
265  } else {
266  if (tuple.x > 0.0) {
267  longitude = tuple.y > 0.0 ? raw_longitude : 2.0 * M_PI - raw_longitude;
268  } else {
269  longitude = tuple.y > 0.0 ? M_PI - raw_longitude : M_PI + raw_longitude;
270  }
271  }
272  if (tuple.z == 0.0) {
273  latitude = 0.0;
274  altitude = distance - Datum::EQUATORIAL_RADIUS;
275  } else {
276  double radius = std::hypot(distance, tuple.z);
277  double inclination = std::asin(tuple.z / radius);
278  double ratio = ECCENTRICITY_SQUARED * Datum::EQUATORIAL_RADIUS / (2.0 * radius);
279 
280  /* Performing iterations. */
281  double latitude_estimate, correction, next_correction = 0.0,
282  sine, root;
283  int iterations = 0;
284  constexpr static int max_iterations = 1000;
285  do {
286  if (iterations > max_iterations) {
287  VSM_EXCEPTION(Exception, "Cannot find fixed point of the transform.");
288  }
289  iterations++;
290  correction = next_correction;
291  latitude_estimate = inclination + correction;
292  sine = std::sin(latitude_estimate);
293  root = std::sqrt(1.0 - ECCENTRICITY_SQUARED * sine * sine);
294  next_correction =
295  std::asin(ratio * std::sin(2.0 * latitude_estimate) / root);
296  } while (std::abs(next_correction - correction) >= threshold);
297 
298  latitude = latitude_estimate;
299  altitude = distance * std::cos(latitude) + tuple.z * sine -
300  Datum::EQUATORIAL_RADIUS * root;
301  }
302  }
303  return Geodetic_tuple(latitude, longitude, altitude);
304  }
305 
306  static Cartesian_tuple
307  Calculate_ecef(const Geodetic_tuple &coord)
308  {
309  double cosine_latitude = std::cos(coord.latitude);
310  double sine_latitude = std::sin(coord.latitude);
311  double cosine_longitude = std::cos(coord.longitude);
312  double sine_longitude = std::sin(coord.longitude);
313 
314  double curvature_radius = Datum::EQUATORIAL_RADIUS /
315  std::sqrt(1.0 - ECCENTRICITY_SQUARED * sine_latitude * sine_latitude);
316 
317  return Cartesian_tuple(
318  (curvature_radius + coord.altitude) * cosine_latitude * cosine_longitude,
319  (curvature_radius + coord.altitude) * cosine_latitude * sine_longitude,
320  ((1.0 - ECCENTRICITY_SQUARED) * curvature_radius + coord.altitude) * sine_latitude
321  );
322  }
323 };
324 
327 
328 float
329 Normalize_angle_0_2pi(float a);
330 
331 float
332 Normalize_angle_minuspi_pi(float a);
333 
334 } /* namespace vsm */
335 } /* namespace ugcs */
336 
337 #endif /* COORDINATES_H_ */
UGCS root namespace.
Definition: android-linux/ugcs/vsm/platform_sockets.h:27
Position(Geodetic_tuple tuple)
Construct position from geodetic coordinates.
Definition: coordinates.h:81
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:183
static constexpr double FLATTENING
Flattening of the reference ellipsoid.
Definition: coordinates.h:31
Position< Wgs84_datum > Wgs84_position
Position defined in WGS84 geodetic system.
Definition: coordinates.h:326
Coordinates tuple for geodetic CS.
Definition: coordinates.h:41
double x
In meters.
Definition: coordinates.h:61
static constexpr double EQUATORIAL_RADIUS
Equatorial radius of the reference ellipsoid, in meters.
Definition: coordinates.h:34
double Earth_radius() const
The Earth&#39;s mean radius of curvature (averaging over all directions) at a latitude of the position...
Definition: coordinates.h:168
VSM exceptions definition.
Geodetic_tuple(double latitude, double longitude, double altitude)
Construct the tuple with specific values.
Definition: coordinates.h:51
Datum for WGS84 geodetic system.
Definition: coordinates.h:28
Position(Cartesian_tuple tuple)
Construct position from ECEF coordinates.
Definition: coordinates.h:89
Datum Datum_type
Associated datum.
Definition: coordinates.h:75
static constexpr double POLAR_RADIUS
Polar radius of the reference ellipsoid, in meters.
Definition: coordinates.h:37
Coordinates tuple for cartesian CS.
Definition: coordinates.h:57
double latitude
Latitude value, radians.
Definition: coordinates.h:44
double Bearing(const Position &target)
Get bearing in radians (-PI; +PI) to the target.
Definition: coordinates.h:144
Cartesian_tuple Get_ecef() const
Get representation in ECEF coordinates - Earth-centered Earth-fixed CS, ((x) axis points from the Ear...
Definition: coordinates.h:107
Immutable position in a specified coordinates system.
Definition: coordinates.h:72
double Long_meter() const
One meter expressed in longitude radians.
Definition: coordinates.h:130
double longitude
Longitude value, radians.
Definition: coordinates.h:44
double Lat_meter() const
One meter expressed in latitude radians.
Definition: coordinates.h:114
Geodetic_tuple Get_geodetic() const
Get representation in geodetic coordinates.
Definition: coordinates.h:95
#define M_PI
PI constant.
Definition: coordinates.h:21
double altitude
Altitude value, meters.
Definition: coordinates.h:44
#define VSM_EXCEPTION(__exc_class, __msg,...)
Throw VSM exception instance.
Definition: exception.h:170
Cartesian_tuple(double x, double y, double z)
Construct the tuple with specific values.
Definition: coordinates.h:65
Base class for all VSM exceptions.
Definition: exception.h:23