20 return degrees * Math::Pi<double>() / 180.0;
24 return radians * 180.0 / Math::Pi<double>();
30 location.
y = -location.
y;
38 auto& p = boost::variant2::get<TransverseMercatorParams>(
params);
43 auto& p = boost::variant2::get<UniversalTransverseMercatorParams>(
params);
48 auto& p = boost::variant2::get<WebMercatorParams>(
params);
53 auto& p = boost::variant2::get<LambertConformalConicParams>(
params);
58 auto& p = boost::variant2::get<TransverseMercatorParams>(
params);
67 auto& p = boost::variant2::get<TransverseMercatorParams>(
params);
72 auto& p = boost::variant2::get<UniversalTransverseMercatorParams>(
params);
77 auto& p = boost::variant2::get<WebMercatorParams>(
params);
82 auto& p = boost::variant2::get<LambertConformalConicParams>(
params);
87 auto& p = boost::variant2::get<TransverseMercatorParams>(
params);
102 double dlon = std::atan2(std::sin(lon - lon_0), std::cos(lon - lon_0));
110 double N = a / std::sqrt(1.0 - e2 * std::sin(lat) * std::sin(lat));
111 double T = std::tan(lat) * std::tan(lat);
112 double C = ep2 * std::cos(lat) * std::cos(lat);
113 double A = std::cos(lat) * dlon;
115 auto meridional_arc = [&](
double phi) {
116 return p.
ellps.
a * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * phi
117 - (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * std::sin(2.0 * phi)
118 + (15.0 * e4 / 256.0 + 45.0 * e6 / 1024.0) * std::sin(4.0 * phi)
119 - (35.0 * e6 / 3072.0) * std::sin(6.0 * phi));
122 double M = meridional_arc(lat);
123 double M_0 = meridional_arc(lat_0);
125 double x = p.
x_0 + p.
k * N * (A + (1.0 - T + C) * std::pow(A, 3) / 6.0
126 + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * ep2) * std::pow(A, 5) / 120.0);
128 double y = p.
y_0 + p.
k * ((M - M_0) + N * std::tan(lat) * ((A*A) / 2.0
129 + (5.0 - T + 9.0 * C + 4.0 * C * C) * std::pow(A, 4) / 24.0
130 + (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * ep2) * std::pow(A, 6) / 720.0));
132 return Location(
static_cast<float>(
x),
static_cast<float>(
y),
static_cast<float>(geolocation.
altitude));
143 double x_0 = 500000.0;
144 double y_0 = (p.
north) ? 0.0 : 10000000.0;
152 double N = a / std::sqrt(1.0 - e2 * std::sin(lat) * std::sin(lat));
153 double T = std::tan(lat) * std::tan(lat);
154 double C = ep2 * std::cos(lat) * std::cos(lat);
155 double A = std::cos(lat) * std::atan2(std::sin(lon - lon_0), std::cos(lon - lon_0));
157 double M = p.
ellps.
a * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * lat
158 - (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * std::sin(2.0 * lat)
159 + (15.0 * e4 / 256.0 + 45.0 * e6 / 1024.0) * std::sin(4.0 * lat)
160 - (35.0 * e6 / 3072.0) * std::sin(6.0 * lat));
162 double x = x_0 + k * N * (A + (1.0 - T + C) * std::pow(A, 3) / 6.0
163 + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * ep2) * std::pow(A, 5) / 120.0);
165 double y = y_0 + k * (M + N * std::tan(lat) * ((A * A) / 2.0
166 + (5.0 - T + 9.0 * C + 4.0 * C * C) * std::pow(A, 4) / 24.0
167 + (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * ep2) * std::pow(A, 6) / 720.0));
169 Location result(
static_cast<float>(
x),
static_cast<float>(
y),
static_cast<float>(geolocation.
altitude));
174 if (p.
offset.has_value()) {
175 result = p.
offset->ApplyInverseTransformation(result);
188 double y = p.
ellps.
a * std::log(std::tan(Math::Pi<double>() / 4.0 + lat / 2.0));
190 return Location(
static_cast<float>(
x),
static_cast<float>(
y),
static_cast<float>(geolocation.
altitude));
205 double e = std::sqrt(e2);
207 double m1 = std::cos(lat_1) / std::sqrt(1.0 - e2 * std::sin(lat_1) * std::sin(lat_1));
208 double m2 = std::cos(lat_2) / std::sqrt(1.0 - e2 * std::sin(lat_2) * std::sin(lat_2));
210 double t0 = std::tan(Math::Pi<double>() / 4 - lat_0 / 2)
211 / std::pow((1.0 - e * std::sin(lat_0)) / (1.0 + e * std::sin(lat_0)), e / 2);
212 double t1 = std::tan(Math::Pi<double>() / 4- lat_1 / 2)
213 / std::pow((1.0 - e * std::sin(lat_1)) / (1.0 + e * std::sin(lat_1)), e / 2);
214 double t2 = std::tan(Math::Pi<double>() / 4 - lat_2 / 2)
215 / std::pow((1.0 - e * std::sin(lat_2)) / (1.0 + e * std::sin(lat_2)), e / 2);
216 double t = std::tan(Math::Pi<double>() / 4 - lat / 2)
217 / std::pow((1.0 - e * std::sin(lat)) / (1.0 + e * std::sin(lat)), e / 2);
219 double n = (std::log(m1) - std::log(m2)) / (std::log(t1) - std::log(t2));
220 double F = m1 / (n * std::pow(t1, n));
221 double rho = a * F * std::pow(t, n);
222 double rho0 = a * F * std::pow(t0, n);
223 double theta = n * std::atan2(std::sin(lon - lon_0), std::cos(lon - lon_0));
225 double x = p.
x_0 + rho * std::sin(theta);
226 double y = p.
y_0 + rho0 - rho * std::cos(theta);
228 return Location(
static_cast<float>(
x),
static_cast<float>(
y),
static_cast<float>(geolocation.
altitude));
244 double x = (location.
x - p.
x_0) / p.
k;
245 double y = (location.
y - p.
y_0) / p.
k;
247 double M = a * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * lat_0
248 - (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * std::sin(2.0 * lat_0)
249 + (15.0 * e4 / 256.0 + 45.0 * e6 / 1024.0) * std::sin(4.0 * lat_0)
250 - (35.0 * e6 / 3072.0) * std::sin(6.0 * lat_0)) +
y;
252 double mu = M / (a * (1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0));
253 double e1 = (1.0 - std::sqrt(1.0 - e2)) / (1.0 + std::sqrt(1.0 - e2));
254 double e1_2 = e1 * e1;
255 double e1_3 = e1_2 * e1;
256 double e1_4 = e1_3 * e1;
258 double phi1 = mu + (3.0 * e1 / 2.0 - 27.0 * e1_3 / 32.0) * std::sin(2.0 * mu)
259 + (21.0 * e1_2 / 16.0 - 55.0 * e1_4 / 32.0) * std::sin(4.0 * mu)
260 + (151.0 * e1_3 / 96.0) * std::sin(6.0 * mu) + (1097.0 * e1_4 / 512.0) * std::sin(8.0 * mu);
262 double sin1 = std::sin(phi1), cos1 = std::cos(phi1), tan1 = std::tan(phi1);
264 double N = a / std::sqrt(1.0 - e2 * sin1 * sin1);
265 double R = a * (1.0 - e2) / std::pow(1.0 - e2 * sin1 * sin1, 1.5);
266 double T = tan1 * tan1;
267 double C = ep2 * cos1 * cos1;
271 double lat = phi1 - (N * tan1 /
R) * ((D * D) / 2.0
272 - (5.0 + 3.0 * T + 10.0 * C - 4.0 * C * C - 9.0 * ep2) * std::pow(D, 4) / 24.0
273 + (61.0 + 90.0 * T + 298.0 * C + 45.0 * T * T - 252.0 * ep2 - 3.0 * C * C) * std::pow(D, 6) / 720.0);
275 double lon = lon_0 + (D - (1.0 + 2.0 * T + C) * std::pow(D, 3) / 6.0
276 + (5.0 - 2.0 * C + 28.0 * T + 3.0 * C * C + 8.0 * ep2 + 24.0 * T * T) * std::pow(D, 5) / 120.0) / cos1;
278 lon = std::atan2(std::sin(lon), std::cos(lon));
289 double x_0 = 500000.0;
290 double y_0 = (p.
north) ? 0.0 : 10000000.0;
299 if (p.
offset.has_value()) {
300 location = p.
offset->ApplyTransformation(location_in);
303 location = location_in;
307 double x = (location.
x - x_0) / k;
308 double y = (location.
y - y_0) / k;
310 double mu =
y / (a * (1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0));
311 double e1 = (1.0 - std::sqrt(1.0 - e2)) / (1.0 + std::sqrt(1.0 - e2));
312 double e1_2 = e1 * e1;
313 double e1_3 = e1_2 * e1;
314 double e1_4 = e1_3 * e1;
316 double phi1 = mu + (3.0 * e1 / 2.0 - 27.0 * e1_3 / 32.0) * std::sin(2.0 * mu)
317 + (21.0 * e1_2 / 16.0 - 55.0 * e1_4 / 32.0) * std::sin(4.0 * mu)
318 + (151.0 * e1_3 / 96.0) * std::sin(6.0 * mu) + (1097.0 * e1_4 / 512.0) * std::sin(8.0 * mu);
320 double sin1 = std::sin(phi1);
321 double cos1 = std::cos(phi1);
322 double tan1 = std::tan(phi1);
324 double N = a / std::sqrt(1.0 - e2 * sin1 * sin1);
325 double R = a * (1.0 - e2) / std::pow(1.0 - e2 * sin1 * sin1, 1.5);
326 double T = tan1 * tan1;
327 double C = ep2 * cos1 * cos1;
330 double lat = phi1 - (N * tan1 /
R) * ((D * D) / 2.0
331 - (5.0 + 3.0 * T + 10.0 * C - 4.0 * C * C - 9.0 * ep2) * std::pow(D, 4) / 24.0
332 + (61.0 + 90.0 * T + 298.0 * C + 45.0 * T * T - 252.0 * ep2 - 3.0 * C * C) * std::pow(D, 6) / 720.0);
334 double lon = lon_0 + (D - (1.0 + 2.0 * T + C) * std::pow(D, 3) / 6.0
335 + (5.0 - 2.0 * C + 28.0 * T + 3.0 * C * C + 8.0 * ep2 + 24.0 * T * T) * std::pow(D, 5) / 120.0) / cos1;
337 lon = std::atan2(std::sin(lon), std::cos(lon));
345 double lon = location.
x / p.
ellps.
a;
346 double lat = 2*std::atan(std::exp(location.
y / p.
ellps.
a)) - Math::Pi<double>()/2;
361 double e = std::sqrt(e2);
363 double m1 = std::cos(lat_1) / std::sqrt(1.0 - e2 * std::sin(lat_1) * std::sin(lat_1));
364 double m2 = std::cos(lat_2) / std::sqrt(1.0 - e2 * std::sin(lat_2) * std::sin(lat_2));
366 double t0 = std::tan(Math::Pi<double>() / 4 - lat_0 / 2)
367 / std::pow((1.0 - e * std::sin(lat_0)) / (1.0 + e * std::sin(lat_0)), e / 2);
368 double t1 = std::tan(Math::Pi<double>() / 4- lat_1 / 2)
369 / std::pow((1.0 - e * std::sin(lat_1)) / (1.0 + e * std::sin(lat_1)), e / 2);
370 double t2 = std::tan(Math::Pi<double>() / 4 - lat_2 / 2)
371 / std::pow((1.0 - e * std::sin(lat_2)) / (1.0 + e * std::sin(lat_2)), e / 2);
373 double n = (std::log(m1) - std::log(m2)) / (std::log(t1) - std::log(t2));
374 double F = m1 / (n * std::pow(t1, n));
375 double rho0 = a * F * std::pow(t0, n);
377 double x =
static_cast<double>(location.
x) - p.
x_0;
378 double y =
static_cast<double>(location.
y) - p.
y_0;
380 double sgn = (n >= 0.0) ? 1.0 : -1.0;
382 double rho = sgn * std::hypot(
x, Y);
383 double theta = std::atan2(sgn *
x, sgn * Y);
385 double t = std::pow(rho / (a * F), 1.0 / n);
388 double lat = Math::Pi<double>() * 0.5 - 2.0 * std::atan(t);
389 for (
int i = 0; i < 10; ++i) {
390 const double lat_next = Math::Pi<double>() * 0.5
391 - 2.0 * std::atan(t * std::pow((1.0 - e * std::sin(lat)) / (1.0 + e * std::sin(lat)), 0.5 * e));
392 if (std::abs(lat_next - lat) < 1e-12){
393 lat = lat_next;
break;
397 double lon = lon_0 + theta / n;
398 lon = std::atan2(std::sin(lon), std::cos(lon));
carla::rpc::Response< T > R
boost::optional< OffsetTransform > offset
static Location SwapHandedness(Location location)
static double RadiansToDegrees(double radians)
ProjectionType
All the supported projection types.
@ UniversalTransverseMercator
static double DegreesToRadians(double degrees)
This file contains definitions of common data structures used in traffic manager.
GeoLocation TransformToGeoLocationUniversalTransverseMercator(const Location &location, const UniversalTransverseMercatorParams) const
Transform the given location to a geo location using UniversalTransverseMercator.
Location GeoLocationToTransformUniversalTransverseMercator(const GeoLocation &geolocation, const UniversalTransverseMercatorParams params) const
Transform the given geo location to a location using UniversalTransverseMercator.
GeoLocation TransformToGeoLocationLambertConformalConic(const Location &location, const LambertConformalConicParams params) const
Transform the given location to a geo location using LambertConformalConic.
GeoLocation TransformToGeoLocationTransverseMercator(const Location &location, const TransverseMercatorParams params) const
Transform the given location to a geo location using TransverseMercator.
Location GeoLocationToTransformLambertConformalConic(const GeoLocation &geolocation, const LambertConformalConicParams params) const
Transform the given geo location to a location using LambertConformalConic.
Location GeoLocationToTransform(const GeoLocation &geolocation) const
Transform the given location to a geo location.
GeoLocation TransformToGeoLocationWebMercator(const Location &location, const WebMercatorParams params) const
Transform the given location to a geo location using WebMercator.
Location GeoLocationToTransformTransverseMercator(const GeoLocation &geolocation, const TransverseMercatorParams params) const
Transform the given geo location to a location using TransverseMercator.
GeoLocation TransformToGeoLocation(const Location &location) const
Transform the given geo location to a location.
Location GeoLocationToTransformWebMercator(const GeoLocation &geolocation, const WebMercatorParams params) const
Transform the given geo location to a location using WebMercator.