| 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 |
} |