CARLA
GeoLocation.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
8 
9 #include "carla/geom/Location.h"
10 #include "carla/geom/Math.h"
11 
12 #include <cmath>
13 
14 #if defined(_WIN32) && !defined(_USE_MATH_DEFINES)
15 # define _USE_MATH_DEFINES
16 # include <math.h> // cmath is not enough for MSVC
17 #endif
18 
19 namespace carla {
20 namespace geom {
21 
22  /// Earth radius at equator [m].
23  static constexpr double EARTH_RADIUS_EQUA = 6378137.0;
24 
25  /// Convert latitude to scale, which is needed by mercator
26  /// transformations
27  /// @param lat latitude in degrees (DEG)
28  /// @return scale factor
29  /// @note when converting from lat/lon -> mercator and back again,
30  /// or vice versa, use the same scale in both transformations!
31  static double LatToScale(double lat) {
32  return std::cos(Math::ToRadians(lat));
33  }
34 
35  /// Converts lat/lon/scale to mx/my (mx/my in meters if correct scale
36  /// is given).
37  template <class float_type>
38  static void LatLonToMercator(double lat, double lon, double scale, float_type &mx, float_type &my) {
39  mx = scale * Math::ToRadians(lon) * EARTH_RADIUS_EQUA;
40  my = scale * EARTH_RADIUS_EQUA * std::log(std::tan((90.0 + lat) * Math::Pi<double>() / 360.0));
41  }
42 
43  /// Converts mx/my/scale to lat/lon (mx/my in meters if correct scale
44  /// is given).
45  static void MercatorToLatLon(double mx, double my, double scale, double &lat, double &lon) {
46  lon = mx * 180.0 / (Math::Pi<double>() * EARTH_RADIUS_EQUA * scale);
47  lat = 360.0 * std::atan(std::exp(my / (EARTH_RADIUS_EQUA * scale))) / Math::Pi<double>() - 90.0;
48  }
49 
50  /// Adds meters dx/dy to given lat/lon and returns new lat/lon.
51  static void LatLonAddMeters(
52  double lat_start,
53  double lon_start,
54  double dx,
55  double dy,
56  double &lat_end,
57  double &lon_end) {
58  double scale = LatToScale(lat_start);
59  double mx, my;
60  LatLonToMercator(lat_start, lon_start, scale, mx, my);
61  mx += dx;
62  my += dy;
63  MercatorToLatLon(mx, my, scale, lat_end, lon_end);
64  }
65 
66  GeoLocation GeoLocation::Transform(const Location &location) const {
67  GeoLocation result{0.0, 0.0, altitude + location.z};
70  location.x, -location.y, // Invert y axis to have increasing latitudes northward
71  result.latitude, result.longitude);
72  return result;
73  }
74 
75 } // namespace geom
76 } // namespace carla
static void LatLonAddMeters(double lat_start, double lon_start, double dx, double dy, double &lat_end, double &lon_end)
Adds meters dx/dy to given lat/lon and returns new lat/lon.
Definition: GeoLocation.cpp:51
static void LatLonToMercator(double lat, double lon, double scale, float_type &mx, float_type &my)
Converts lat/lon/scale to mx/my (mx/my in meters if correct scale is given).
Definition: GeoLocation.cpp:38
static void log(Args &&... args)
Definition: Logging.h:59
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
static constexpr double EARTH_RADIUS_EQUA
Earth radius at equator [m].
Definition: GeoLocation.cpp:23
GeoLocation Transform(const Location &location) const
Transform the given location to a GeoLocation using this as geo-reference.
Definition: GeoLocation.cpp:66
static void MercatorToLatLon(double mx, double my, double scale, double &lat, double &lon)
Converts mx/my/scale to lat/lon (mx/my in meters if correct scale is given).
Definition: GeoLocation.cpp:45
static constexpr T ToRadians(T deg)
Definition: Math.h:43
static double LatToScale(double lat)
Convert latitude to scale, which is needed by mercator transformations.
Definition: GeoLocation.cpp:31