text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          https://www.mrpt.org/                         |
     |                                                                        |
     | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
     | See: https://www.mrpt.org/Authors - All rights reserved.               |
     | Released under BSD License. See: https://www.mrpt.org/License          |
     +------------------------------------------------------------------------+ */
  
  #include "topography-precomp.h"  // Precompiled headers
  
  #include <mrpt/math/geometry.h>
  #include <mrpt/math/utils.h>
  #include <mrpt/poses/CPoint3D.h>
  #include <mrpt/poses/CPose3D.h>
  #include <mrpt/topography/conversions.h>
  
  using namespace std;
  using namespace mrpt;
  using namespace mrpt::math;
  using namespace mrpt::poses;
  
  bool mrpt::topography::operator==(const TCoords& a, const TCoords& o)
  {
  	return a.decimal_value == o.decimal_value;
  }
  bool mrpt::topography::operator!=(const TCoords& a, const TCoords& o)
  {
  	return !(a == o);
  }
  bool mrpt::topography::operator==(
  	const TGeodeticCoords& a, const TGeodeticCoords& o)
  {
  	return a.lat == o.lat && a.lon == o.lon && a.height == o.height;
  }
  bool mrpt::topography::operator!=(
  	const TGeodeticCoords& a, const TGeodeticCoords& o)
  {
  	return !(a == o);
  }
  
  //#define M_PI_TOPO 3.141592653589793
  // inline double DEG2RAD(const double x) { return x*M_PI_TOPO/180.0;	}
  
  // Define a generic type for "precission numbers":
  #ifdef HAVE_LONG_DOUBLE
  using precnum_t = long double;
  #else
  using precnum_t = double;
  #endif
  
  std::ostream& mrpt::topography::operator<<(std::ostream& out, const TCoords& o)
  {
  	return out << o.getAsString();
  }
  
  /*---------------------------------------------------------------
  				geocentricToENU_WGS84
   ---------------------------------------------------------------*/
  void mrpt::topography::geocentricToENU_WGS84(
  	const mrpt::math::TPoint3D& P_geocentric_,
  	mrpt::math::TPoint3D& out_ENU_point,
  	const TGeodeticCoords& in_coords_origin)
  {
  	// Generate reference 3D point:
  	TPoint3D P_geocentric_ref;
  	geodeticToGeocentric_WGS84(in_coords_origin, P_geocentric_ref);
  
  	const double clat = cos(DEG2RAD(in_coords_origin.lat)),
  				 slat = sin(DEG2RAD(in_coords_origin.lat));
  	const double clon = cos(DEG2RAD(in_coords_origin.lon)),
  				 slon = sin(DEG2RAD(in_coords_origin.lon));
  
  	// Compute the resulting relative coordinates:
  	// For using smaller numbers:
  	const mrpt::math::TPoint3D P_geocentric = P_geocentric_ - P_geocentric_ref;
  
  	// Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
  	//   after rotation given by the transposed rotation matrix from ENU ->
  	//   ECEF.
  	out_ENU_point.x = -slon * P_geocentric.x + clon * P_geocentric.y;
  	out_ENU_point.y = -clon * slat * P_geocentric.x -
  					  slon * slat * P_geocentric.y + clat * P_geocentric.z;
  	out_ENU_point.z = clon * clat * P_geocentric.x +
  					  slon * clat * P_geocentric.y + slat * P_geocentric.z;
  }
  
  void mrpt::topography::geocentricToENU_WGS84(
  	const std::vector<mrpt::math::TPoint3D>& in_geocentric_points,
  	std::vector<mrpt::math::TPoint3D>& out_ENU_points,
  	const TGeodeticCoords& in_coords_origin)
  {
  	// Generate reference 3D point:
  	TPoint3D P_geocentric_ref;
  	geodeticToGeocentric_WGS84(in_coords_origin, P_geocentric_ref);
  
  	const double clat = cos(DEG2RAD(in_coords_origin.lat)),
  				 slat = sin(DEG2RAD(in_coords_origin.lat));
  	const double clon = cos(DEG2RAD(in_coords_origin.lon)),
  				 slon = sin(DEG2RAD(in_coords_origin.lon));
  
  	const size_t N = in_geocentric_points.size();
  	out_ENU_points.resize(N);
  	for (size_t i = 0; i < N; i++)
  	{
  		// Compute the resulting relative coordinates for using smaller numbers:
  		const mrpt::math::TPoint3D P_geocentric =
  			in_geocentric_points[i] - P_geocentric_ref;
  
  		// Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
  		//   after rotation given by the transposed rotation matrix from ENU ->
  		//   ECEF.
  		out_ENU_points[i].x = -slon * P_geocentric.x + clon * P_geocentric.y;
  		out_ENU_points[i].y = -clon * slat * P_geocentric.x -
  							  slon * slat * P_geocentric.y +
  							  clat * P_geocentric.z;
  		out_ENU_points[i].z = clon * clat * P_geocentric.x +
  							  slon * clat * P_geocentric.y +
  							  slat * P_geocentric.z;
  	}
  }
  
  /*---------------------------------------------------------------
  				geodeticToENU_WGS84
   ---------------------------------------------------------------*/
  void mrpt::topography::geodeticToENU_WGS84(
  	const TGeodeticCoords& in_coords, mrpt::math::TPoint3D& out_ENU_point,
  	const TGeodeticCoords& in_coords_origin)
  {
  	// --------------------------------------------------------------------
  	//  Explanation: We compute the earth-centric coordinates first,
  	//    then make a system transformation to local XYZ coordinates
  	//    using a system of three orthogonal vectors as local reference.
  	//
  	// See: http://en.wikipedia.org/wiki/Reference_ellipsoid
  	// (JLBC 21/DEC/2006)  (Fixed: JLBC 9/JUL/2008)
  	// - Oct/2013, Emilio Sanjurjo: Fixed UP vector pointing exactly normal to
  	// ellipsoid surface.
  	// --------------------------------------------------------------------
  	// Generate 3D point:
  	TPoint3D P_geocentric;
  	geodeticToGeocentric_WGS84(in_coords, P_geocentric);
  
  	geocentricToENU_WGS84(P_geocentric, out_ENU_point, in_coords_origin);
  }
  
  /*---------------------------------------------------------------
  					ENU_axes_from_WGS84
   ---------------------------------------------------------------*/
  void mrpt::topography::ENU_axes_from_WGS84(
  	double in_longitude_reference_degrees, double in_latitude_reference_degrees,
  	double in_height_reference_meters, mrpt::math::TPose3D& out_ENU,
  	bool only_angles)
  {
  	// See "coordinatesTransformation_WGS84" for more comments.
  	TPoint3D PPref;
  	mrpt::topography::geodeticToGeocentric_WGS84(
  		TGeodeticCoords(
  			in_latitude_reference_degrees, in_longitude_reference_degrees,
  			in_height_reference_meters),
  		PPref);
  
  	const double clat = cos(DEG2RAD(in_latitude_reference_degrees)),
  				 slat = sin(DEG2RAD(in_latitude_reference_degrees));
  	const double clon = cos(DEG2RAD(in_longitude_reference_degrees)),
  				 slon = sin(DEG2RAD(in_longitude_reference_degrees));
  
  	CMatrixDouble44 HM;  // zeros by default
  	HM(0, 0) = -slon;
  	HM(0, 1) = -clon * slat;
  	HM(0, 2) = clon * clat;
  	HM(1, 0) = clon;
  	HM(1, 1) = -slon * slat;
  	HM(1, 2) = slon * clat;
  	HM(2, 0) = 0;
  	HM(2, 1) = clat;
  	HM(2, 2) = slat;
  	HM(3, 3) = 1;
  
  	if (!only_angles)
  	{
  		HM(0, 3) = PPref.x;
  		HM(1, 3) = PPref.y;
  		HM(2, 3) = PPref.z;
  	}
  
  	out_ENU = CPose3D(HM).asTPose();
  }
  
  //*---------------------------------------------------------------
  //			geodeticToGeocentric_WGS84
  // ---------------------------------------------------------------*/
  void mrpt::topography::geodeticToGeocentric_WGS84(
  	const TGeodeticCoords& in_coords, mrpt::math::TPoint3D& out_point)
  {
  	// --------------------------------------------------------------------
  	// See: http://en.wikipedia.org/wiki/Reference_ellipsoid
  	//  Constants are for WGS84
  	// --------------------------------------------------------------------
  
  	static const precnum_t a =
  		6378137L;  // Semi-major axis of the Earth (meters)
  	static const precnum_t b = 6356752.3142L;  // Semi-minor axis:
  
  	static const precnum_t ae = acos(b / a);  // eccentricity:
  	static const precnum_t cos2_ae_earth =
  		square(cos(ae));  // The cos^2 of the angular eccentricity of the Earth:
  	// // 0.993305619995739L;
  	static const precnum_t sin2_ae_earth =
  		square(sin(ae));  // The sin^2 of the angular eccentricity of the Earth:
  	// // 0.006694380004261L;
  
  	const precnum_t lon = DEG2RAD(precnum_t(in_coords.lon));
  	const precnum_t lat = DEG2RAD(precnum_t(in_coords.lat));
  
  	// The radius of curvature in the prime vertical:
  	const precnum_t N = a / std::sqrt(1 - sin2_ae_earth * square(sin(lat)));
  
  	// Generate 3D point:
  	out_point.x =
  		static_cast<double>((N + in_coords.height) * cos(lat) * cos(lon));
  	out_point.y =
  		static_cast<double>((N + in_coords.height) * cos(lat) * sin(lon));
  	out_point.z =
  		static_cast<double>((cos2_ae_earth * N + in_coords.height) * sin(lat));
  }
  
  /*---------------------------------------------------------------
  			geodeticToGeocentric_WGS84
   ---------------------------------------------------------------*/
  void mrpt::topography::geodeticToGeocentric(
  	const TGeodeticCoords& in_coords, TGeocentricCoords& out_point,
  	const TEllipsoid& ellip)
  {
  	static const precnum_t a =
  		ellip.sa;  // Semi-major axis of the Earth (meters)
  	static const precnum_t b = ellip.sb;  // Semi-minor axis:
  
  	static const precnum_t ae = acos(b / a);  // eccentricity:
  	static const precnum_t cos2_ae_earth =
  		square(cos(ae));  // The cos^2 of the angular eccentricity of the Earth:
  	// // 0.993305619995739L;
  	static const precnum_t sin2_ae_earth =
  		square(sin(ae));  // The sin^2 of the angular eccentricity of the Earth:
  	// // 0.006694380004261L;
  
  	const precnum_t lon = DEG2RAD(precnum_t(in_coords.lon));
  	const precnum_t lat = DEG2RAD(precnum_t(in_coords.lat));
  
  	// The radius of curvature in the prime vertical:
  	const precnum_t N = a / std::sqrt(1 - sin2_ae_earth * square(sin(lat)));
  
  	// Generate 3D point:
  	out_point.x =
  		static_cast<double>((N + in_coords.height) * cos(lat) * cos(lon));
  	out_point.y =
  		static_cast<double>((N + in_coords.height) * cos(lat) * sin(lon));
  	out_point.z =
  		static_cast<double>((cos2_ae_earth * N + in_coords.height) * sin(lat));
  }
  
  /*---------------------------------------------------------------
  			geocentricToGeodetic
   ---------------------------------------------------------------*/
  void mrpt::topography::geocentricToGeodetic(
  	const TGeocentricCoords& in_point, TGeodeticCoords& out_coords,
  	const TEllipsoid& ellip)
  {
  	const double sa2 = ellip.sa * ellip.sa;
  	const double sb2 = ellip.sb * ellip.sb;
  
  	const double e2 = (sa2 - sb2) / sa2;
  	const double ep2 = (sa2 - sb2) / sb2;
  	const double p = sqrt(in_point.x * in_point.x + in_point.y * in_point.y);
  	const double theta = atan2(in_point.z * ellip.sa, p * ellip.sb);
  
  	out_coords.lon = atan2(in_point.y, in_point.x);
  	out_coords.lat = atan2(
  		in_point.z + ep2 * ellip.sb * sin(theta) * sin(theta) * sin(theta),
  		p - e2 * ellip.sa * cos(theta) * cos(theta) * cos(theta));
  
  	const double clat = cos(out_coords.lat);
  	const double slat = sin(out_coords.lat);
  	const double N = sa2 / sqrt(sa2 * clat * clat + sb2 * slat * slat);
  
  	out_coords.height = p / clat - N;
  
  	out_coords.lon = RAD2DEG(out_coords.lon);
  	out_coords.lat = RAD2DEG(out_coords.lat);
  }
  
  /*---------------------------------------------------------------
  					UTMToGeodesic
   ---------------------------------------------------------------*/
  void mrpt::topography::UTMToGeodetic(
  	double X, double Y, int huso, char hem, double& out_lon /*degrees*/,
  	double& out_lat /*degrees*/, const TEllipsoid& ellip)
  {
  	ASSERT_(hem == 's' || hem == 'S' || hem == 'n' || hem == 'N');
  
  	X = X - 5e5;
  	if (hem == 's' || hem == 'S') Y = Y - 1e7;
  
  	const precnum_t lon0 = huso * 6 - 183;
  	const precnum_t a2 = ellip.sa * ellip.sa;
  	const precnum_t b2 = ellip.sb * ellip.sb;
  	const precnum_t ep2 = (a2 - b2) / b2;
  	const precnum_t c = a2 / ellip.sb;
  	const precnum_t latp = Y / (6366197.724 * 0.9996);
  	const precnum_t clp2 = square(cos(latp));
  
  	const precnum_t v = c * 0.9996 / sqrt(1 + ep2 * clp2);
  	const precnum_t na = X / v;
  	const precnum_t A1 = sin(2 * latp);
  	const precnum_t A2 = A1 * clp2;
  	const precnum_t J2 = latp + A1 * 0.5;
  	const precnum_t J4 = 0.75 * J2 + 0.25 * A2;
  	const precnum_t J6 = (5 * J4 + A2 * clp2) / 3;
  
  	const precnum_t alp = 0.75 * ep2;
  	const precnum_t beta = (5.0 / 3.0) * alp * alp;
  	const precnum_t gam = (35.0 / 27.0) * alp * alp * alp;
  	const precnum_t B = 0.9996 * c * (latp - alp * J2 + beta * J4 - gam * J6);
  	const precnum_t nb = (Y - B) / v;
  	const precnum_t psi = (ep2 * square(na)) / 2 * clp2;
  	const precnum_t eps = na * (1 - psi / 3);
  	const precnum_t nu = nb * (1 - psi) + latp;
  	const precnum_t she = (exp(eps) - exp(-eps)) / 2;
  	const precnum_t dlon = atan2(she, cos(nu));
  	const precnum_t tau = atan2(cos(dlon) * tan(nu), 1);
  
  	out_lon = static_cast<double>(RAD2DEG(dlon) + lon0);
  	out_lat = static_cast<double>(RAD2DEG(
  		latp +
  		(1 + ep2 * clp2 - 1.5 * ep2 * sin(latp) * cos(latp) * (tau - latp)) *
  			(tau - latp)));
  }
  
  void mrpt::topography::geodeticToUTM(
  	const TGeodeticCoords& GeodeticCoords, TUTMCoords& UTMCoords, int& UTMZone,
  	char& UTMLatitudeBand, const TEllipsoid& ellip)
  {
  	const double la = GeodeticCoords.lat;
  	char Letra;
  	if (la < -72)
  		Letra = 'C';
  	else if (la < -64)
  		Letra = 'D';
  	else if (la < -56)
  		Letra = 'E';
  	else if (la < -48)
  		Letra = 'F';
  	else if (la < -40)
  		Letra = 'G';
  	else if (la < -32)
  		Letra = 'H';
  	else if (la < -24)
  		Letra = 'J';
  	else if (la < -16)
  		Letra = 'K';
  	else if (la < -8)
  		Letra = 'L';
  	else if (la < 0)
  		Letra = 'M';
  	else if (la < 8)
  		Letra = 'N';
  	else if (la < 16)
  		Letra = 'P';
  	else if (la < 24)
  		Letra = 'Q';
  	else if (la < 32)
  		Letra = 'R';
  	else if (la < 40)
  		Letra = 'S';
  	else if (la < 48)
  		Letra = 'T';
  	else if (la < 56)
  		Letra = 'U';
  	else if (la < 64)
  		Letra = 'V';
  	else if (la < 72)
  		Letra = 'W';
  	else
  		Letra = 'X';
  
  	const precnum_t lat = DEG2RAD(GeodeticCoords.lat);
  	const precnum_t lon = DEG2RAD(GeodeticCoords.lon);
  	const int Huso = mrpt::fix((GeodeticCoords.lon / 6) + 31);
  	const precnum_t lon0 = DEG2RAD(Huso * 6 - 183);
  
  	const precnum_t sa = ellip.sa;
  	const precnum_t sb = ellip.sb;
  	//	const precnum_t e2		= (sa*sa-sb*sb)/(sa*sa);
  	const precnum_t ep2 = (sa * sa - sb * sb) / (sb * sb);
  	const precnum_t c = sa * sa / sb;
  	//	const precnum_t alp		= (sa-sb)/sa;
  
  	const precnum_t Dlon = lon - lon0;
  	const precnum_t clat = cos(lat);
  	const precnum_t sDlon = sin(Dlon);
  	const precnum_t cDlon = cos(Dlon);
  
  	const precnum_t A = clat * sDlon;
  	const precnum_t eps = 0.5 * log((1 + A) / (1 - A));
  	const precnum_t nu = atan2(tan(lat), cDlon) - lat;
  	const precnum_t v = 0.9996 * c / sqrt(1 + ep2 * clat * clat);
  	const precnum_t psi = 0.5 * ep2 * eps * eps * clat * clat;
  	const precnum_t A1 = sin(2 * lat);
  	const precnum_t A2 = A1 * clat * clat;
  	const precnum_t J2 = lat + 0.5 * A1;
  	const precnum_t J4 = 0.75 * J2 + 0.25 * A2;
  	const precnum_t J6 = (5.0 * J4 + A2 * clat * clat) / 3;
  	const precnum_t nalp = 0.75 * ep2;
  	const precnum_t nbet = (5.0 / 3.0) * nalp * nalp;
  	const precnum_t ngam = (35.0 / 27.0) * nalp * nalp * nalp;
  	const precnum_t B = 0.9996 * c * (lat - nalp * J2 + nbet * J4 - ngam * J6);
  
  	UTMCoords.x = static_cast<double>(eps * v * (1 + psi / 3.0) + 500000);
  	UTMCoords.y = static_cast<double>(nu * v * (1 + psi) + B);
  	UTMCoords.z = static_cast<double>(GeodeticCoords.height);
  
  	UTMZone = Huso;
  	UTMLatitudeBand = Letra;
  }
  
  /*---------------------------------------------------------------
  					LatLonToUTM
   ---------------------------------------------------------------*/
  void mrpt::topography::GeodeticToUTM(
  	double la, double lo, double& xx, double& yy, int& out_UTM_zone,
  	char& out_UTM_latitude_band, const TEllipsoid& ellip)
  {
  	// This method is based on public code by Gabriel Ruiz Martinez and Rafael
  	// Palacios.
  	//  http://www.mathworks.com/matlabcentral/fileexchange/10915
  
  	const double sa = ellip.sa;
  	const double sb = ellip.sb;
  	const double e2 = (sqrt((sa * sa) - (sb * sb))) / sb;
  
  	const double e2cuadrada = e2 * e2;
  
  	const double c = (sa * sa) / sb;
  
  	const double lat = DEG2RAD(la);
  	const double lon = DEG2RAD(lo);
  
  	const int Huso = mrpt::fix((lo / 6) + 31);
  	double S = ((Huso * 6) - 183);
  	double deltaS = lon - DEG2RAD(S);
  
  	char Letra;
  
  	if (la < -72)
  		Letra = 'C';
  	else if (la < -64)
  		Letra = 'D';
  	else if (la < -56)
  		Letra = 'E';
  	else if (la < -48)
  		Letra = 'F';
  	else if (la < -40)
  		Letra = 'G';
  	else if (la < -32)
  		Letra = 'H';
  	else if (la < -24)
  		Letra = 'J';
  	else if (la < -16)
  		Letra = 'K';
  	else if (la < -8)
  		Letra = 'L';
  	else if (la < 0)
  		Letra = 'M';
  	else if (la < 8)
  		Letra = 'N';
  	else if (la < 16)
  		Letra = 'P';
  	else if (la < 24)
  		Letra = 'Q';
  	else if (la < 32)
  		Letra = 'R';
  	else if (la < 40)
  		Letra = 'S';
  	else if (la < 48)
  		Letra = 'T';
  	else if (la < 56)
  		Letra = 'U';
  	else if (la < 64)
  		Letra = 'V';
  	else if (la < 72)
  		Letra = 'W';
  	else
  		Letra = 'X';
  
  	const double a = cos(lat) * sin(deltaS);
  	const double epsilon = 0.5 * log((1 + a) / (1 - a));
  	const double nu = atan(tan(lat) / cos(deltaS)) - lat;
  	const double v = (c / sqrt((1 + (e2cuadrada * square(cos(lat)))))) * 0.9996;
  	const double ta = 0.5 * e2cuadrada * square(epsilon) * square(cos(lat));
  	const double a1 = sin(2 * lat);
  	const double a2 = a1 * square(cos(lat));
  	const double j2 = lat + 0.5 * a1;
  	const double j4 = ((3.0 * j2) + a2) / 4.0;
  	const double j6 = ((5.0 * j4) + (a2 * square(cos(lat)))) / 3.0;
  	const double alfa = 0.75 * e2cuadrada;
  	const double beta = (5.0 / 3.0) * pow(alfa, 2.0);
  	const double gama = (35.0 / 27.0) * pow(alfa, 3.0);
  	const double Bm = 0.9996 * c * (lat - alfa * j2 + beta * j4 - gama * j6);
  
  	xx = epsilon * v * (1 + (ta / 3.0)) + 500000;
  	yy = nu * v * (1 + ta) + Bm;
  
  	if (yy < 0) yy += 9999999;
  
  	out_UTM_zone = Huso;
  	out_UTM_latitude_band = Letra;
  }
  
  /**  7-parameter Bursa-Wolf transformation:
   *   [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
   * ] [ X Y Z ]_local
   * \sa transform10params
   */
  void mrpt::topography::transform7params(
  	const mrpt::math::TPoint3D& p, const TDatum7Params& d,
  	mrpt::math::TPoint3D& o)
  {
  	const double scale = (1 + d.dS);
  
  	o.x = d.dX + scale * (p.x + p.y * d.Rz - p.z * d.Ry);
  	o.y = d.dY + scale * (-p.x * d.Rz + p.y + p.z * d.Rx);
  	o.z = d.dZ + scale * (p.x * d.Ry - p.y * d.Rx + p.z);
  }
  
  /**  7-parameter Bursa-Wolf transformation TOPCON:
   *   [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
   * ] [ X Y Z ]_local
   * \sa transform10params
   */
  void mrpt::topography::transform7params_TOPCON(
  	const mrpt::math::TPoint3D& p, const TDatum7Params_TOPCON& d,
  	mrpt::math::TPoint3D& o)
  {
  	const double scale = (1 + d.dS);
  
  	o.x = d.dX + scale * (d.m11 * p.x + d.m12 * p.y + d.m13 * p.z);
  	o.y = d.dY + scale * (d.m21 * p.x + d.m22 * p.y + d.m23 * p.z);
  	o.z = d.dZ + scale * (d.m31 * p.x + d.m32 * p.y + d.m33 * p.z);
  }
  
  /**  10-parameter Molodensky-Badekas transformation:
   *   [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
   * ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
   * \sa transform7params
   */
  void mrpt::topography::transform10params(
  	const mrpt::math::TPoint3D& p, const TDatum10Params& d,
  	mrpt::math::TPoint3D& o)
  {
  	const double scale = (1 + d.dS);
  
  	const double px = p.x - d.Xp;
  	const double py = p.y - d.Yp;
  	const double pz = p.z - d.Zp;
  
  	o.x = d.dX + scale * (px + py * d.Rz - pz * d.Ry) + d.Xp;
  	o.y = d.dY + scale * (-px * d.Rz + py + pz * d.Rx) + d.Yp;
  	o.z = d.dZ + scale * (px * d.Ry - py * d.Rx + pz) + d.Zp;
  }
  
  /**  Helmert 2D transformation:
   *   [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
   * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
   * \sa transformHelmert3D
   */
  void mrpt::topography::transformHelmert2D(
  	const mrpt::math::TPoint2D& p, const TDatumHelmert2D& d,
  	mrpt::math::TPoint2D& o)
  {
  	const double scale = (1 + d.dS);
  
  	const double px = p.x - d.Xp;
  	const double py = p.y - d.Yp;
  
  	o.x = d.dX + scale * (px * cos(d.alpha) - py * sin(d.alpha)) + d.Xp;
  	o.y = d.dY + scale * (px * sin(d.alpha) + py * cos(d.alpha)) + d.Yp;
  }
  
  /**  Helmert 2D transformation:
   *   [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
   * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
   * \sa transformHelmert3D
   */
  void mrpt::topography::transformHelmert2D_TOPCON(
  	const mrpt::math::TPoint2D& p, const TDatumHelmert2D_TOPCON& d,
  	mrpt::math::TPoint2D& o)
  {
  	o.x = d.a * p.x + d.b * p.y + d.c;
  	o.y = -d.b * p.x + d.a * p.y + d.d;
  }
  
  /**  Helmert 3D transformation:
   *   [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
   * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
   * \sa transformHelmert3D
   */
  void mrpt::topography::transformHelmert3D(
  	const mrpt::math::TPoint3D& p, const TDatumHelmert3D& d,
  	mrpt::math::TPoint3D& o)
  {
  	TDatum7Params d2(d.dX, d.dY, d.dZ, -1 * d.Rx, -1 * d.Ry, -1 * d.Rz, d.dS);
  	transform7params(p, d2, o);
  }
  
  /**  Helmert 3D transformation:
   *   [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
   * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
   * \sa transformHelmert3D
   */
  void mrpt::topography::transformHelmert3D_TOPCON(
  	const mrpt::math::TPoint3D& p, const TDatumHelmert3D_TOPCON& d,
  	mrpt::math::TPoint3D& o)
  {
  	o.x = d.a * p.x + d.b * p.y + d.c;
  	o.y = d.d * p.x + d.e * p.y + d.f;
  	o.z = p.z + d.g;
  }
  
  /**  1D transformation:
   *   [ Z ]_WGS84 = (dy * X - dx * Y + Z)*(1+e)+DZ
   * \sa transformHelmert3D
   */
  void mrpt::topography::transform1D(
  	const mrpt::math::TPoint3D& p, const TDatum1DTransf& d,
  	mrpt::math::TPoint3D& o)
  {
  	o.x = p.x;
  	o.y = p.y;
  	o.z = (d.dY * p.x - d.dX * p.y + p.z) * (1 + d.dS) + d.DZ;
  }
  
  /**  1D transformation:
   *   [ X;Y ]_WGS84 = [X;Y]_locales+[1 -sin(d.beta);0
   * cos(d.beta)]*[x*d.dSx;y*d.dSy ]
   * \sa transformHelmert3D
   */
  void mrpt::topography::transfInterpolation(
  	const mrpt::math::TPoint3D& p, const TDatumTransfInterpolation& d,
  	mrpt::math::TPoint3D& o)
  {
  	o.x = d.dX + p.x * d.dSx - p.y * d.dSy * sin(d.beta);
  	o.y = d.dY + p.y * d.dSy * cos(d.beta);
  	o.z = p.z;
  }
  
  /** ENU to geocentric coordinates.
   * \sa geodeticToENU_WGS84
   */
  void mrpt::topography::ENUToGeocentric(
  	const mrpt::math::TPoint3D& p, const TGeodeticCoords& in_coords_origin,
  	TGeocentricCoords& out_coords, const TEllipsoid& ellip)
  {
  	// Generate reference 3D point:
  	TPoint3D P_geocentric_ref;
  	mrpt::topography::geodeticToGeocentric(
  		in_coords_origin, P_geocentric_ref, ellip);
  
  	CVectorDouble P_ref(3);
  	P_ref[0] = P_geocentric_ref.x;
  	P_ref[1] = P_geocentric_ref.y;
  	P_ref[2] = P_geocentric_ref.z;
  
  	// Z axis -> In direction out-ward the center of the Earth:
  	CVectorDouble REF_X(3), REF_Y(3), REF_Z(3);
  	math::normalize(P_ref, REF_Z);
  
  	// 1st column: Starting at the reference point, move in the tangent
  	// direction
  	//   east-ward: I compute this as the derivative of P_ref wrt "longitude":
  	//      A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon);  --> -Z[1]
  	//      A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon);  -->  Z[0]
  	//      A_east[2] = 0;                                       -->  0
  	// ---------------------------------------------------------------------------
  	CVectorDouble AUX_X(3);
  	AUX_X[0] = -REF_Z[1];
  	AUX_X[1] = REF_Z[0];
  	AUX_X[2] = 0;
  	math::normalize(AUX_X, REF_X);
  
  	// 2nd column: The cross product:
  	math::crossProduct3D(REF_Z, REF_X, REF_Y);
  
  	out_coords.x =
  		REF_X[0] * p.x + REF_Y[0] * p.y + REF_Z[0] * p.z + P_geocentric_ref.x;
  	out_coords.y =
  		REF_X[1] * p.x + REF_Y[1] * p.y + REF_Z[1] * p.z + P_geocentric_ref.y;
  	out_coords.z =
  		REF_X[2] * p.x + REF_Y[2] * p.y + REF_Z[2] * p.z + P_geocentric_ref.z;
  }
  