Debian Codegraph @

libs/topography/src/conversions.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "topography-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/math/geometry.h>
13 #include <mrpt/math/utils.h>
14 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/topography/conversions.h>
17
18 using namespace std;
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace mrpt::poses;
22
23 bool mrpt::topography::operator==(const TCoords& a, const TCoords& o)
24 {
25 return a.decimal_value == o.decimal_value;
26 }
27 bool mrpt::topography::operator!=(const TCoords& a, const TCoords& o) { return !(a == o); }
28 bool mrpt::topography::operator==(const TGeodeticCoords& a, const TGeodeticCoords& o)
29 {
30 return a.lat == o.lat && a.lon == o.lon && a.height == o.height;
31 }
32 bool mrpt::topography::operator!=(const TGeodeticCoords& a, const TGeodeticCoords& o)
33 {
34 return !(a == o);
35 }
36
37 //#define M_PI_TOPO 3.141592653589793
38 // inline double DEG2RAD(const double x) { return x*M_PI_TOPO/180.0; }
39
40 // Define a generic type for "precission numbers":
41 #ifdef HAVE_LONG_DOUBLE
42 using precnum_t = long double;
43 #else
44 using precnum_t = double;
45 #endif
46
47 std::ostream& mrpt::topography::operator<<(std::ostream& out, const TCoords& o)
48 {
49 return out << o.getAsString();
50 }
51
52 /*---------------------------------------------------------------
53 geocentricToENU_WGS84
54 ---------------------------------------------------------------*/
55 void mrpt::topography::geocentricToENU_WGS84(
56 const mrpt::math::TPoint3D& P_geocentric_,
57 mrpt::math::TPoint3D& out_ENU_point,
58 const TGeodeticCoords& in_coords_origin)
59 {
60 // Generate reference 3D point:
61 TPoint3D P_geocentric_ref;
62 geodeticToGeocentric_WGS84(in_coords_origin, P_geocentric_ref);
63
64 const double clat = cos(DEG2RAD(in_coords_origin.lat)), slat = sin(DEG2RAD(in_coords_origin.lat));
65 const double clon = cos(DEG2RAD(in_coords_origin.lon)), slon = sin(DEG2RAD(in_coords_origin.lon));
66
67 // Compute the resulting relative coordinates:
68 // For using smaller numbers:
69 const mrpt::math::TPoint3D P_geocentric = P_geocentric_ - P_geocentric_ref;
70
71 // Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
72 // after rotation given by the transposed rotation matrix from ENU ->
73 // ECEF.
74 out_ENU_point.x = -slon * P_geocentric.x + clon * P_geocentric.y;
75 out_ENU_point.y =
76 -clon * slat * P_geocentric.x - slon * slat * P_geocentric.y + clat * P_geocentric.z;
77 out_ENU_point.z =
78 clon * clat * P_geocentric.x + slon * clat * P_geocentric.y + slat * P_geocentric.z;
79 }
80
81 void mrpt::topography::geocentricToENU_WGS84(
82 const std::vector<mrpt::math::TPoint3D>& in_geocentric_points,
83 std::vector<mrpt::math::TPoint3D>& out_ENU_points,
84 const TGeodeticCoords& in_coords_origin)
85 {
86 // Generate reference 3D point:
87 TPoint3D P_geocentric_ref;
88 geodeticToGeocentric_WGS84(in_coords_origin, P_geocentric_ref);
89
90 const double clat = cos(DEG2RAD(in_coords_origin.lat)), slat = sin(DEG2RAD(in_coords_origin.lat));
91 const double clon = cos(DEG2RAD(in_coords_origin.lon)), slon = sin(DEG2RAD(in_coords_origin.lon));
92
93 const size_t N = in_geocentric_points.size();
94 out_ENU_points.resize(N);
95 for (size_t i = 0; i < N; i++)
96 {
97 // Compute the resulting relative coordinates for using smaller numbers:
98 const mrpt::math::TPoint3D P_geocentric = in_geocentric_points[i] - P_geocentric_ref;
99
100 // Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
101 // after rotation given by the transposed rotation matrix from ENU ->
102 // ECEF.
103 out_ENU_points[i].x = -slon * P_geocentric.x + clon * P_geocentric.y;
104 out_ENU_points[i].y =
105 -clon * slat * P_geocentric.x - slon * slat * P_geocentric.y + clat * P_geocentric.z;
106 out_ENU_points[i].z =
107 clon * clat * P_geocentric.x + slon * clat * P_geocentric.y + slat * P_geocentric.z;
108 }
109 }
110
111 /*---------------------------------------------------------------
112 geodeticToENU_WGS84
113 ---------------------------------------------------------------*/
114 void mrpt::topography::geodeticToENU_WGS84(
115 const TGeodeticCoords& in_coords,
116 mrpt::math::TPoint3D& out_ENU_point,
117 const TGeodeticCoords& in_coords_origin)
118 {
119 // --------------------------------------------------------------------
120 // Explanation: We compute the earth-centric coordinates first,
121 // then make a system transformation to local XYZ coordinates
122 // using a system of three orthogonal vectors as local reference.
123 //
124 // See: http://en.wikipedia.org/wiki/Reference_ellipsoid
125 // (JLBC 21/DEC/2006) (Fixed: JLBC 9/JUL/2008)
126 // - Oct/2013, Emilio Sanjurjo: Fixed UP vector pointing exactly normal to
127 // ellipsoid surface.
128 // --------------------------------------------------------------------
129 // Generate 3D point:
130 TPoint3D P_geocentric;
131 geodeticToGeocentric_WGS84(in_coords, P_geocentric);
132
133 geocentricToENU_WGS84(P_geocentric, out_ENU_point, in_coords_origin);
134 }
135
136 /*---------------------------------------------------------------
137 ENU_axes_from_WGS84
138 ---------------------------------------------------------------*/
139 void mrpt::topography::ENU_axes_from_WGS84(
140 double in_longitude_reference_degrees,
141 double in_latitude_reference_degrees,
142 double in_height_reference_meters,
143 mrpt::math::TPose3D& out_ENU,
144 bool only_angles)
145 {
146 // See "coordinatesTransformation_WGS84" for more comments.
147 TPoint3D PPref;
148 mrpt::topography::geodeticToGeocentric_WGS84(
149 TGeodeticCoords(
150 in_latitude_reference_degrees, in_longitude_reference_degrees,
151 in_height_reference_meters),
152 PPref);
153
154 const double clat = cos(DEG2RAD(in_latitude_reference_degrees)),
155 slat = sin(DEG2RAD(in_latitude_reference_degrees));
156 const double clon = cos(DEG2RAD(in_longitude_reference_degrees)),
157 slon = sin(DEG2RAD(in_longitude_reference_degrees));
158
159 CMatrixDouble44 HM; // zeros by default
160 HM(0, 0) = -slon;
161 HM(0, 1) = -clon * slat;
162 HM(0, 2) = clon * clat;
163 HM(1, 0) = clon;
164 HM(1, 1) = -slon * slat;
165 HM(1, 2) = slon * clat;
166 HM(2, 0) = 0;
167 HM(2, 1) = clat;
168 HM(2, 2) = slat;
169 HM(3, 3) = 1;
170
171 if (!only_angles)
172 {
173 HM(0, 3) = PPref.x;
174 HM(1, 3) = PPref.y;
175 HM(2, 3) = PPref.z;
176 }
177
178 out_ENU = CPose3D(HM).asTPose();
179 }
180
181 //*---------------------------------------------------------------
182 // geodeticToGeocentric_WGS84
183 // ---------------------------------------------------------------*/
184 void mrpt::topography::geodeticToGeocentric_WGS84(
185 const TGeodeticCoords& in_coords, mrpt::math::TPoint3D& out_point)
186 {
187 // --------------------------------------------------------------------
188 // See: http://en.wikipedia.org/wiki/Reference_ellipsoid
189 // Constants are for WGS84
190 // --------------------------------------------------------------------
191
192 static const precnum_t a = 6378137L; // Semi-major axis of the Earth (meters)
193 static const precnum_t b = 6356752.3142L; // Semi-minor axis:
194
195 static const precnum_t ae = acos(b / a); // eccentricity:
196 static const precnum_t cos2_ae_earth =
197 square(cos(ae)); // The cos^2 of the angular eccentricity of the Earth:
198 // // 0.993305619995739L;
199 static const precnum_t sin2_ae_earth =
200 square(sin(ae)); // The sin^2 of the angular eccentricity of the Earth:
201 // // 0.006694380004261L;
202
203 const precnum_t lon = DEG2RAD(precnum_t(in_coords.lon));
204 const precnum_t lat = DEG2RAD(precnum_t(in_coords.lat));
205
206 // The radius of curvature in the prime vertical:
207 const precnum_t N = a / std::sqrt(1 - sin2_ae_earth * square(sin(lat)));
208
209 // Generate 3D point:
210 out_point.x = static_cast<double>((N + in_coords.height) * cos(lat) * cos(lon));
211 out_point.y = static_cast<double>((N + in_coords.height) * cos(lat) * sin(lon));
212 out_point.z = static_cast<double>((cos2_ae_earth * N + in_coords.height) * sin(lat));
213 }
214
215 /*---------------------------------------------------------------
216 geodeticToGeocentric_WGS84
217 ---------------------------------------------------------------*/
218 void mrpt::topography::geodeticToGeocentric(
219 const TGeodeticCoords& in_coords, TGeocentricCoords& out_point, const TEllipsoid& ellip)
220 {
221 static const precnum_t a = ellip.sa; // Semi-major axis of the Earth (meters)
222 static const precnum_t b = ellip.sb; // Semi-minor axis:
223
224 static const precnum_t ae = acos(b / a); // eccentricity:
225 static const precnum_t cos2_ae_earth =
226 square(cos(ae)); // The cos^2 of the angular eccentricity of the Earth:
227 // // 0.993305619995739L;
228 static const precnum_t sin2_ae_earth =
229 square(sin(ae)); // The sin^2 of the angular eccentricity of the Earth:
230 // // 0.006694380004261L;
231
232 const precnum_t lon = DEG2RAD(precnum_t(in_coords.lon));
233 const precnum_t lat = DEG2RAD(precnum_t(in_coords.lat));
234
235 // The radius of curvature in the prime vertical:
236 const precnum_t N = a / std::sqrt(1 - sin2_ae_earth * square(sin(lat)));
237
238 // Generate 3D point:
239 out_point.x = static_cast<double>((N + in_coords.height) * cos(lat) * cos(lon));
240 out_point.y = static_cast<double>((N + in_coords.height) * cos(lat) * sin(lon));
241 out_point.z = static_cast<double>((cos2_ae_earth * N + in_coords.height) * sin(lat));
242 }
243
244 /*---------------------------------------------------------------
245 geocentricToGeodetic
246 ---------------------------------------------------------------*/
247 void mrpt::topography::geocentricToGeodetic(
248 const TGeocentricCoords& in_point, TGeodeticCoords& out_coords, const TEllipsoid& ellip)
249 {
250 const double sa2 = ellip.sa * ellip.sa;
251 const double sb2 = ellip.sb * ellip.sb;
252
253 const double e2 = (sa2 - sb2) / sa2;
254 const double ep2 = (sa2 - sb2) / sb2;
255 const double p = sqrt(in_point.x * in_point.x + in_point.y * in_point.y);
256 const double theta = atan2(in_point.z * ellip.sa, p * ellip.sb);
257
258 out_coords.lon = atan2(in_point.y, in_point.x);
259 out_coords.lat = atan2(
260 in_point.z + ep2 * ellip.sb * sin(theta) * sin(theta) * sin(theta),
261 p - e2 * ellip.sa * cos(theta) * cos(theta) * cos(theta));
262
263 const double clat = cos(out_coords.lat);
264 const double slat = sin(out_coords.lat);
265 const double N = sa2 / sqrt(sa2 * clat * clat + sb2 * slat * slat);
266
267 out_coords.height = p / clat - N;
268
269 out_coords.lon = RAD2DEG(out_coords.lon);
270 out_coords.lat = RAD2DEG(out_coords.lat);
271 }
272
273 /*---------------------------------------------------------------
274 UTMToGeodesic
275 ---------------------------------------------------------------*/
276 void mrpt::topography::UTMToGeodetic(
277 double X,
278 double Y,
279 int huso,
280 char hem,
281 double& out_lon /*degrees*/,
282 double& out_lat /*degrees*/,
283 const TEllipsoid& ellip)
284 {
285 ASSERT_(hem == 's' || hem == 'S' || hem == 'n' || hem == 'N');
286
287 X = X - 5e5;
288 if (hem == 's' || hem == 'S') Y = Y - 1e7;
289
290 const precnum_t lon0 = huso * 6 - 183;
291 const precnum_t a2 = ellip.sa * ellip.sa;
292 const precnum_t b2 = ellip.sb * ellip.sb;
293 const precnum_t ep2 = (a2 - b2) / b2;
294 const precnum_t c = a2 / ellip.sb;
295 const precnum_t latp = Y / (6366197.724 * 0.9996);
296 const precnum_t clp2 = square(cos(latp));
297
298 const precnum_t v = c * 0.9996 / sqrt(1 + ep2 * clp2);
299 const precnum_t na = X / v;
300 const precnum_t A1 = sin(2 * latp);
301 const precnum_t A2 = A1 * clp2;
302 const precnum_t J2 = latp + A1 * 0.5;
303 const precnum_t J4 = 0.75 * J2 + 0.25 * A2;
304 const precnum_t J6 = (5 * J4 + A2 * clp2) / 3;
305
306 const precnum_t alp = 0.75 * ep2;
307 const precnum_t beta = (5.0 / 3.0) * alp * alp;
308 const precnum_t gam = (35.0 / 27.0) * alp * alp * alp;
309 const precnum_t B = 0.9996 * c * (latp - alp * J2 + beta * J4 - gam * J6);
310 const precnum_t nb = (Y - B) / v;
311 const precnum_t psi = (ep2 * square(na)) / 2 * clp2;
312 const precnum_t eps = na * (1 - psi / 3);
313 const precnum_t nu = nb * (1 - psi) + latp;
314 const precnum_t she = (exp(eps) - exp(-eps)) / 2;
315 const precnum_t dlon = atan2(she, cos(nu));
316 const precnum_t tau = atan2(cos(dlon) * tan(nu), 1);
317
318 out_lon = static_cast<double>(RAD2DEG(dlon) + lon0);
319 out_lat = static_cast<double>(RAD2DEG(
320 latp + (1 + ep2 * clp2 - 1.5 * ep2 * sin(latp) * cos(latp) * (tau - latp)) * (tau - latp)));
321 }
322
323 void mrpt::topography::geodeticToUTM(
324 const TGeodeticCoords& GeodeticCoords,
325 TUTMCoords& UTMCoords,
326 int& UTMZone,
327 char& UTMLatitudeBand,
328 const TEllipsoid& ellip)
329 {
330 const double la = GeodeticCoords.lat;
331 char Letra;
332 if (la < -72)
333 Letra = 'C';
334 else if (la < -64)
335 Letra = 'D';
336 else if (la < -56)
337 Letra = 'E';
338 else if (la < -48)
339 Letra = 'F';
340 else if (la < -40)
341 Letra = 'G';
342 else if (la < -32)
343 Letra = 'H';
344 else if (la < -24)
345 Letra = 'J';
346 else if (la < -16)
347 Letra = 'K';
348 else if (la < -8)
349 Letra = 'L';
350 else if (la < 0)
351 Letra = 'M';
352 else if (la < 8)
353 Letra = 'N';
354 else if (la < 16)
355 Letra = 'P';
356 else if (la < 24)
357 Letra = 'Q';
358 else if (la < 32)
359 Letra = 'R';
360 else if (la < 40)
361 Letra = 'S';
362 else if (la < 48)
363 Letra = 'T';
364 else if (la < 56)
365 Letra = 'U';
366 else if (la < 64)
367 Letra = 'V';
368 else if (la < 72)
369 Letra = 'W';
370 else
371 Letra = 'X';
372
373 const precnum_t lat = DEG2RAD(GeodeticCoords.lat);
374 const precnum_t lon = DEG2RAD(GeodeticCoords.lon);
375 const int Huso = mrpt::fix((GeodeticCoords.lon / 6) + 31);
376 const precnum_t lon0 = DEG2RAD(Huso * 6 - 183);
377
378 const precnum_t sa = ellip.sa;
379 const precnum_t sb = ellip.sb;
380 // const precnum_t e2 = (sa*sa-sb*sb)/(sa*sa);
381 const precnum_t ep2 = (sa * sa - sb * sb) / (sb * sb);
382 const precnum_t c = sa * sa / sb;
383 // const precnum_t alp = (sa-sb)/sa;
384
385 const precnum_t Dlon = lon - lon0;
386 const precnum_t clat = cos(lat);
387 const precnum_t sDlon = sin(Dlon);
388 const precnum_t cDlon = cos(Dlon);
389
390 const precnum_t A = clat * sDlon;
391 const precnum_t eps = 0.5 * log((1 + A) / (1 - A));
392 const precnum_t nu = atan2(tan(lat), cDlon) - lat;
393 const precnum_t v = 0.9996 * c / sqrt(1 + ep2 * clat * clat);
394 const precnum_t psi = 0.5 * ep2 * eps * eps * clat * clat;
395 const precnum_t A1 = sin(2 * lat);
396 const precnum_t A2 = A1 * clat * clat;
397 const precnum_t J2 = lat + 0.5 * A1;
398 const precnum_t J4 = 0.75 * J2 + 0.25 * A2;
399 const precnum_t J6 = (5.0 * J4 + A2 * clat * clat) / 3;
400 const precnum_t nalp = 0.75 * ep2;
401 const precnum_t nbet = (5.0 / 3.0) * nalp * nalp;
402 const precnum_t ngam = (35.0 / 27.0) * nalp * nalp * nalp;
403 const precnum_t B = 0.9996 * c * (lat - nalp * J2 + nbet * J4 - ngam * J6);
404
405 UTMCoords.x = static_cast<double>(eps * v * (1 + psi / 3.0) + 500000);
406 UTMCoords.y = static_cast<double>(nu * v * (1 + psi) + B);
407 UTMCoords.z = static_cast<double>(GeodeticCoords.height);
408
409 UTMZone = Huso;
410 UTMLatitudeBand = Letra;
411 }
412
413 /*---------------------------------------------------------------
414 LatLonToUTM
415 ---------------------------------------------------------------*/
416 void mrpt::topography::GeodeticToUTM(
417 double la,
418 double lo,
419 double& xx,
420 double& yy,
421 int& out_UTM_zone,
422 char& out_UTM_latitude_band,
423 const TEllipsoid& ellip)
424 {
425 // This method is based on public code by Gabriel Ruiz Martinez and Rafael
426 // Palacios.
427 // http://www.mathworks.com/matlabcentral/fileexchange/10915
428
429 const double sa = ellip.sa;
430 const double sb = ellip.sb;
431 const double e2 = (sqrt((sa * sa) - (sb * sb))) / sb;
432
433 const double e2cuadrada = e2 * e2;
434
435 const double c = (sa * sa) / sb;
436
437 const double lat = DEG2RAD(la);
438 const double lon = DEG2RAD(lo);
439
440 const int Huso = mrpt::fix((lo / 6) + 31);
441 double S = ((Huso * 6) - 183);
442 double deltaS = lon - DEG2RAD(S);
443
444 char Letra;
445
446 if (la < -72)
447 Letra = 'C';
448 else if (la < -64)
449 Letra = 'D';
450 else if (la < -56)
451 Letra = 'E';
452 else if (la < -48)
453 Letra = 'F';
454 else if (la < -40)
455 Letra = 'G';
456 else if (la < -32)
457 Letra = 'H';
458 else if (la < -24)
459 Letra = 'J';
460 else if (la < -16)
461 Letra = 'K';
462 else if (la < -8)
463 Letra = 'L';
464 else if (la < 0)
465 Letra = 'M';
466 else if (la < 8)
467 Letra = 'N';
468 else if (la < 16)
469 Letra = 'P';
470 else if (la < 24)
471 Letra = 'Q';
472 else if (la < 32)
473 Letra = 'R';
474 else if (la < 40)
475 Letra = 'S';
476 else if (la < 48)
477 Letra = 'T';
478 else if (la < 56)
479 Letra = 'U';
480 else if (la < 64)
481 Letra = 'V';
482 else if (la < 72)
483 Letra = 'W';
484 else
485 Letra = 'X';
486
487 const double a = cos(lat) * sin(deltaS);
488 const double epsilon = 0.5 * log((1 + a) / (1 - a));
489 const double nu = atan(tan(lat) / cos(deltaS)) - lat;
490 const double v = (c / sqrt((1 + (e2cuadrada * square(cos(lat)))))) * 0.9996;
491 const double ta = 0.5 * e2cuadrada * square(epsilon) * square(cos(lat));
492 const double a1 = sin(2 * lat);
493 const double a2 = a1 * square(cos(lat));
494 const double j2 = lat + 0.5 * a1;
495 const double j4 = ((3.0 * j2) + a2) / 4.0;
496 const double j6 = ((5.0 * j4) + (a2 * square(cos(lat)))) / 3.0;
497 const double alfa = 0.75 * e2cuadrada;
498 const double beta = (5.0 / 3.0) * pow(alfa, 2.0);
499 const double gama = (35.0 / 27.0) * pow(alfa, 3.0);
500 const double Bm = 0.9996 * c * (lat - alfa * j2 + beta * j4 - gama * j6);
501
502 xx = epsilon * v * (1 + (ta / 3.0)) + 500000;
503 yy = nu * v * (1 + ta) + Bm;
504
505 if (yy < 0) yy += 9999999;
506
507 out_UTM_zone = Huso;
508 out_UTM_latitude_band = Letra;
509 }
510
511 /** 7-parameter Bursa-Wolf transformation:
512 * [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
513 * ] [ X Y Z ]_local
514 * \sa transform10params
515 */
516 void mrpt::topography::transform7params(
517 const mrpt::math::TPoint3D& p, const TDatum7Params& d, mrpt::math::TPoint3D& o)
518 {
519 const double scale = (1 + d.dS);
520
521 o.x = d.dX + scale * (p.x + p.y * d.Rz - p.z * d.Ry);
522 o.y = d.dY + scale * (-p.x * d.Rz + p.y + p.z * d.Rx);
523 o.z = d.dZ + scale * (p.x * d.Ry - p.y * d.Rx + p.z);
524 }
525
526 /** 7-parameter Bursa-Wolf transformation TOPCON:
527 * [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
528 * ] [ X Y Z ]_local
529 * \sa transform10params
530 */
531 void mrpt::topography::transform7params_TOPCON(
532 const mrpt::math::TPoint3D& p, const TDatum7Params_TOPCON& d, mrpt::math::TPoint3D& o)
533 {
534 const double scale = (1 + d.dS);
535
536 o.x = d.dX + scale * (d.m11 * p.x + d.m12 * p.y + d.m13 * p.z);
537 o.y = d.dY + scale * (d.m21 * p.x + d.m22 * p.y + d.m23 * p.z);
538 o.z = d.dZ + scale * (d.m31 * p.x + d.m32 * p.y + d.m33 * p.z);
539 }
540
541 /** 10-parameter Molodensky-Badekas transformation:
542 * [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
543 * ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
544 * \sa transform7params
545 */
546 void mrpt::topography::transform10params(
547 const mrpt::math::TPoint3D& p, const TDatum10Params& d, mrpt::math::TPoint3D& o)
548 {
549 const double scale = (1 + d.dS);
550
551 const double px = p.x - d.Xp;
552 const double py = p.y - d.Yp;
553 const double pz = p.z - d.Zp;
554
555 o.x = d.dX + scale * (px + py * d.Rz - pz * d.Ry) + d.Xp;
556 o.y = d.dY + scale * (-px * d.Rz + py + pz * d.Rx) + d.Yp;
557 o.z = d.dZ + scale * (px * d.Ry - py * d.Rx + pz) + d.Zp;
558 }
559
560 /** Helmert 2D transformation:
561 * [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
562 * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
563 * \sa transformHelmert3D
564 */
565 void mrpt::topography::transformHelmert2D(
566 const mrpt::math::TPoint2D& p, const TDatumHelmert2D& d, mrpt::math::TPoint2D& o)
567 {
568 const double scale = (1 + d.dS);
569
570 const double px = p.x - d.Xp;
571 const double py = p.y - d.Yp;
572
573 o.x = d.dX + scale * (px * cos(d.alpha) - py * sin(d.alpha)) + d.Xp;
574 o.y = d.dY + scale * (px * sin(d.alpha) + py * cos(d.alpha)) + d.Yp;
575 }
576
577 /** Helmert 2D transformation:
578 * [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
579 * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
580 * \sa transformHelmert3D
581 */
582 void mrpt::topography::transformHelmert2D_TOPCON(
583 const mrpt::math::TPoint2D& p, const TDatumHelmert2D_TOPCON& d, mrpt::math::TPoint2D& o)
584 {
585 o.x = d.a * p.x + d.b * p.y + d.c;
586 o.y = -d.b * p.x + d.a * p.y + d.d;
587 }
588
589 /** Helmert 3D transformation:
590 * [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
591 * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
592 * \sa transformHelmert3D
593 */
594 void mrpt::topography::transformHelmert3D(
595 const mrpt::math::TPoint3D& p, const TDatumHelmert3D& d, mrpt::math::TPoint3D& o)
596 {
597 TDatum7Params d2(d.dX, d.dY, d.dZ, -1 * d.Rx, -1 * d.Ry, -1 * d.Rz, d.dS);
598 transform7params(p, d2, o);
599 }
600
601 /** Helmert 3D transformation:
602 * [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
603 * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
604 * \sa transformHelmert3D
605 */
606 void mrpt::topography::transformHelmert3D_TOPCON(
607 const mrpt::math::TPoint3D& p, const TDatumHelmert3D_TOPCON& d, mrpt::math::TPoint3D& o)
608 {
609 o.x = d.a * p.x + d.b * p.y + d.c;
610 o.y = d.d * p.x + d.e * p.y + d.f;
611 o.z = p.z + d.g;
612 }
613
614 /** 1D transformation:
615 * [ Z ]_WGS84 = (dy * X - dx * Y + Z)*(1+e)+DZ
616 * \sa transformHelmert3D
617 */
618 void mrpt::topography::transform1D(
619 const mrpt::math::TPoint3D& p, const TDatum1DTransf& d, mrpt::math::TPoint3D& o)
620 {
621 o.x = p.x;
622 o.y = p.y;
623 o.z = (d.dY * p.x - d.dX * p.y + p.z) * (1 + d.dS) + d.DZ;
624 }
625
626 /** 1D transformation:
627 * [ X;Y ]_WGS84 = [X;Y]_locales+[1 -sin(d.beta);0
628 * cos(d.beta)]*[x*d.dSx;y*d.dSy ]
629 * \sa transformHelmert3D
630 */
631 void mrpt::topography::transfInterpolation(
632 const mrpt::math::TPoint3D& p, const TDatumTransfInterpolation& d, mrpt::math::TPoint3D& o)
633 {
634 o.x = d.dX + p.x * d.dSx - p.y * d.dSy * sin(d.beta);
635 o.y = d.dY + p.y * d.dSy * cos(d.beta);
636 o.z = p.z;
637 }
638
639 /** ENU to geocentric coordinates.
640 * \sa geodeticToENU_WGS84
641 */
642 void mrpt::topography::ENUToGeocentric(
643 const mrpt::math::TPoint3D& p,
644 const TGeodeticCoords& in_coords_origin,
645 TGeocentricCoords& out_coords,
646 const TEllipsoid& ellip)
647 {
648 // Generate reference 3D point:
649 TPoint3D P_geocentric_ref;
650 mrpt::topography::geodeticToGeocentric(in_coords_origin, P_geocentric_ref, ellip);
651
652 CVectorDouble P_ref(3);
653 P_ref[0] = P_geocentric_ref.x;
654 P_ref[1] = P_geocentric_ref.y;
655 P_ref[2] = P_geocentric_ref.z;
656
657 // Z axis -> In direction out-ward the center of the Earth:
658 CVectorDouble REF_X(3), REF_Y(3), REF_Z(3);
659 math::normalize(P_ref, REF_Z);
660
661 // 1st column: Starting at the reference point, move in the tangent
662 // direction
663 // east-ward: I compute this as the derivative of P_ref wrt "longitude":
664 // A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon); --> -Z[1]
665 // A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon); --> Z[0]
666 // A_east[2] = 0; --> 0
667 // ---------------------------------------------------------------------------
668 CVectorDouble AUX_X(3);
669 AUX_X[0] = -REF_Z[1];
670 AUX_X[1] = REF_Z[0];
671 AUX_X[2] = 0;
672 math::normalize(AUX_X, REF_X);
673
674 // 2nd column: The cross product:
675 math::crossProduct3D(REF_Z, REF_X, REF_Y);
676
677 out_coords.x = REF_X[0] * p.x + REF_Y[0] * p.y + REF_Z[0] * p.z + P_geocentric_ref.x;
678 out_coords.y = REF_X[1] * p.x + REF_Y[1] * p.y + REF_Z[1] * p.z + P_geocentric_ref.y;
679 out_coords.z = REF_X[2] * p.x + REF_Y[2] * p.y + REF_Z[2] * p.z + P_geocentric_ref.z;
680 }