Added support for EPSG:9606 and EPSG:9607 datum transformations

gpxsee.pro

@@ -126,7 +126,8 @@ HEADERS += src/config.h \
     src/data/igcparser.h \
     src/data/nmeaparser.h \
     src/data/oziparsers.h \
-    src/map/rectd.h
+    src/map/rectd.h \
+    src/map/geocentric.h
 SOURCES += src/main.cpp \
     src/common/coordinates.cpp \
     src/common/rectc.cpp \
@@ -220,7 +221,8 @@ SOURCES += src/main.cpp \
     src/data/fitparser.cpp \
     src/data/igcparser.cpp \
     src/data/nmeaparser.cpp \
-    src/data/oziparsers.cpp
+    src/data/oziparsers.cpp \
+    src/map/geocentric.cpp
 RESOURCES += gpxsee.qrc
 TRANSLATIONS = lang/gpxsee_cs.ts \
     lang/gpxsee_sv.ts \

src/map/datum.cpp

@@ -2,6 +2,10 @@
 #include "common/wgs84.h"
 #include "datum.h"
 
+
+#define as2rad(x) ((x) * 4.84814e-6)
+#define rad2as(x) ((x) * 206265.0)
+
 static Ellipsoid WGS84e = Ellipsoid(WGS84_RADIUS, WGS84_FLATTENING);
 static Datum WGS84 = Datum(&WGS84e, 0.0, 0.0, 0.0);
 
@@ -43,29 +47,103 @@ static Coordinates molodensky(const Coordinates &c, const Datum &from,
 	return Coordinates(c.lon() + rad2deg(dlon), c.lat() + rad2deg(dlat));
 }
 
-Datum::Datum(const Ellipsoid *ellipsoid, double dx, double dy, double dz)
+
-  : _ellipsoid(ellipsoid), _dx(dx), _dy(dy), _dz(dz)
+Point3D Datum::helmert(const Point3D &p) const
 {
-	_WGS84 = (_ellipsoid->radius() == WGS84_RADIUS
+	double scale = 1 + _ds * 1e-6;
-	  && _ellipsoid->flattening() == WGS84_FLATTENING && _dx == 0.0
+	double R00 = 1;
-	  && _dy == 0.0 && _dz == 0.0) ? true : false;
+	double R01 = _rz;
+	double R02 = -_ry;
+	double R10 = -_rz;
+	double R11 = 1;
+	double R12 = _rx;
+	double R20 = _ry;
+	double R21 = -_rx;
+	double R22 = 1;
+
+	return Point3D(scale * (R00 * p.x() + R01 * p.y() + R02 * p.z()) + _dx,
+	  scale * (R10 * p.x() + R11 * p.y() + R12 * p.z()) + _dy,
+	  scale * (R20 * p.x() + R21 * p.y() + R22 * p.z()) + _dz);
+}
+
+Point3D Datum::helmertr(const Point3D &p) const
+{
+	double scale = 1 + _ds * 1e-6;
+	double R00 = 1;
+	double R01 = _rz;
+	double R02 = -_ry;
+	double R10 = -_rz;
+	double R11 = 1;
+	double R12 = _rx;
+	double R20 = _ry;
+	double R21 = -_rx;
+	double R22 = 1;
+
+	double x = (p.x() - _dx) / scale;
+	double y = (p.y() - _dy) / scale;
+	double z = (p.z() - _dz) / scale;
+
+	return Point3D(R00 * x + R10 * y + R20 * z, R01 * x + R11 * y + R21 * z,
+	  R02 * x + R12 * y + R22 * z);
+}
+
+Datum::Datum(const Ellipsoid *ellipsoid, double dx, double dy, double dz,
+  double rx, double ry, double rz, double ds)
+  : _ellipsoid(ellipsoid), _dx(dx), _dy(dy), _dz(dz), _rx(as2rad(rx)),
+  _ry(as2rad(ry)), _rz(as2rad(rz)), _ds(ds)
+{
+	if (_ellipsoid->radius() == WGS84_RADIUS && _ellipsoid->flattening()
+	  == WGS84_FLATTENING && _dx == 0.0 && _dy == 0.0 && _dz == 0.0
+	  && _rx == 0.0 && _ry == 0.0 && _rz == 0.0 && _ds == 0.0)
+		_shift = None;
+	else
+		_shift = Helmert;
+}
+
+Datum::Datum(const Ellipsoid *ellipsoid, double dx, double dy, double dz)
+  : _ellipsoid(ellipsoid), _dx(dx), _dy(dy), _dz(dz), _rx(0.0), _ry(0.0),
+  _rz(0.0), _ds(0.0)
+{
+	if (_ellipsoid->radius() == WGS84_RADIUS && _ellipsoid->flattening()
+	  == WGS84_FLATTENING && _dx == 0.0 && _dy == 0.0 && _dz == 0.0)
+		_shift = None;
+	else
+		_shift = Molodensky;
 }
 
 Coordinates Datum::toWGS84(const Coordinates &c) const
 {
-	return _WGS84 ? c : molodensky(c, *this, WGS84);
+	switch (_shift) {
+		case Helmert:
+			return Geocentric::toGeodetic(helmert(Geocentric::fromGeodetic(c,
+			  *this)), WGS84);
+		case Molodensky:
+			return molodensky(c, *this, WGS84);
+		default:
+			return c;
+	}
 }
 
 Coordinates Datum::fromWGS84(const Coordinates &c) const
 {
-	return _WGS84 ? c : molodensky(c, WGS84, *this);
+	switch (_shift) {
+		case Helmert:
+			return Geocentric::toGeodetic(helmertr(Geocentric::fromGeodetic(c,
+			  WGS84)), *this);
+		case Molodensky:
+			return molodensky(c, WGS84, *this);
+		default:
+			return c;
+	}
 }
 
 #ifndef QT_NO_DEBUG
 QDebug operator<<(QDebug dbg, const Datum &datum)
 {
 	dbg.nospace() << "Datum(" << *datum.ellipsoid() << ", " << datum.dx()
-	  << ", " << datum.dy() << ", " << datum.dz() << ")";
+	  << ", " << datum.dy() << ", " << datum.dz() << ", " << rad2as(datum.rx())
+	  << ", " << rad2as(datum.ry()) << ", " << rad2as(datum.rz()) << ", "
+	  << datum.ds() << ")";
 	return dbg.space();
 }
 #endif // QT_NO_DEBUG

src/map/datum.h

@@ -4,40 +4,57 @@
 #include <cmath>
 #include <QList>
 #include <QDebug>
-#include "ellipsoid.h"
 #include "common/coordinates.h"
+#include "ellipsoid.h"
+#include "geocentric.h"
 
 class Datum
 {
 public:
-	Datum() : _ellipsoid(0), _dx(NAN), _dy(NAN), _dz(NAN),
+	Datum() : _ellipsoid(0), _shift(None), _dx(NAN), _dy(NAN), _dz(NAN),
-	  _WGS84(false) {}
+	  _rx(NAN), _ry(NAN), _rz(NAN), _ds(NAN) {}
+	Datum(const Ellipsoid *ellipsoid, double dx, double dy, double dz,
+	  double rx, double ry, double rz, double ds);
 	Datum(const Ellipsoid *ellipsoid, double dx, double dy, double dz);
 
 	const Ellipsoid *ellipsoid() const {return _ellipsoid;}
 	double dx() const {return _dx;}
 	double dy() const {return _dy;}
 	double dz() const {return _dz;}
+	double ds() const {return _ds;}
+	double rx() const {return _rx;}
+	double ry() const {return _ry;}
+	double rz() const {return _rz;}
 
 	bool isNull() const
-	  {return (!_ellipsoid && std::isnan(_dx) && std::isnan(_dy)
+	  {return !_ellipsoid;}
-	    && std::isnan(_dz));}
 	bool isValid() const
 	  {return (_ellipsoid && !std::isnan(_dx) && !std::isnan(_dy)
-	    && !std::isnan(_dz));}
+		&& !std::isnan(_dz) && !std::isnan(_ds) && !std::isnan(_rx)
+		&& !std::isnan(_ry) && !std::isnan(_rz));}
 
 	Coordinates toWGS84(const Coordinates &c) const;
 	Coordinates fromWGS84(const Coordinates &c) const;
 
 private:
+	enum ShiftType {
+		None,
+		Molodensky,
+		Helmert
+	};
+
+	Point3D helmert(const Point3D &p) const;
+	Point3D helmertr(const Point3D &p) const;
+
 	const Ellipsoid *_ellipsoid;
-	double _dx, _dy, _dz;
+	ShiftType _shift;
-	bool _WGS84;
+	double _dx, _dy, _dz, _rx, _ry, _rz, _ds;
 };
 
 inline bool operator==(const Datum &d1, const Datum &d2)
-  {return (d1.ellipsoid() == d2.ellipsoid() && d1.dx() == d2.dx()
+  {return (*d1.ellipsoid() == *d2.ellipsoid() && d1.dx() == d2.dx()
-    && d1.dy() == d2.dy() && d1.dz() == d2.dz());}
+	&& d1.dy() == d2.dy() && d1.dz() == d2.dz() && d1.rx() == d2.rx()
+	&& d1.ry() == d2.ry() && d1.rz() == d2.rz() && d1.ds() == d2.ds());}
 
 #ifndef QT_NO_DEBUG
 QDebug operator<<(QDebug dbg, const Datum &datum);

src/map/gcs.cpp

@@ -78,6 +78,7 @@ void GCS::loadList(const QString &path)
 	bool res;
 	int ln = 0;
 	const Ellipsoid *e;
+	double ds, rx, ry, rz;
 
 
 	if (!file.open(QFile::ReadOnly)) {
@@ -145,6 +146,33 @@ void GCS::loadList(const QString &path)
 			qWarning("%s:%d: Invalid dz", qPrintable(path), ln);
 			continue;
 		}
+		if (list.size() == 14) {
+			rx = list[10].trimmed().toDouble(&res);
+			if (!res) {
+				qWarning("%s:%d: Invalid rx", qPrintable(path), ln);
+				continue;
+			}
+			ry = list[11].trimmed().toDouble(&res);
+			if (!res) {
+				qWarning("%s:%d: Invalid ry", qPrintable(path), ln);
+				continue;
+			}
+			rz = list[12].trimmed().toDouble(&res);
+			if (!res) {
+				qWarning("%s:%d: Invalid rz", qPrintable(path), ln);
+				continue;
+			}
+			ds = list[13].trimmed().toDouble(&res);
+			if (!res) {
+				qWarning("%s:%d: Invalid ds", qPrintable(path), ln);
+				continue;
+			}
+		} else {
+			rx = NAN;
+			ry = NAN;
+			rz = NAN;
+			ds = NAN;
+		}
 
 		if (!(e = Ellipsoid::ellipsoid(el))) {
 			qWarning("%s:%d: Unknown ellipsoid code", qPrintable(path), ln);
@@ -156,11 +184,22 @@ void GCS::loadList(const QString &path)
 			case 9603:
 				datum = Datum(e, dx, dy, dz);
 				break;
+			case 9606:
+				datum = Datum(e, dx, dy, dz, -rx, -ry, -rz, ds);
+				break;
+			case 9607:
+				datum = Datum(e, dx, dy, dz, rx, ry, rz, ds);
+				break;
 			default:
 				qWarning("%s:%d: Unknown coordinates transformation method",
 				  qPrintable(path), ln);
 				continue;
 		}
+		if (!datum.isValid()) {
+			qWarning("%s:%d: Invalid coordinates transformation parameters",
+			  qPrintable(path), ln);
+			continue;
+		}
 
 		GCS gcs(datum, pm, au);
 		if (gcs.isValid())

src/map/geocentric.cpp

@@ -0,0 +1,118 @@
+/*
+ * Based on libgeotrans with the following Source Code Disclaimer:
+
+1. The GEOTRANS source code ("the software") is provided free of charge by
+the National Imagery and Mapping Agency (NIMA) of the United States
+Department of Defense. Although NIMA makes no copyright claim under Title 17
+U.S.C., NIMA claims copyrights in the source code under other legal regimes.
+NIMA hereby grants to each user of the software a license to use and
+distribute the software, and develop derivative works.
+
+2. Warranty Disclaimer: The software was developed to meet only the internal
+requirements of the U.S. National Imagery and Mapping Agency. The software
+is provided "as is," and no warranty, express or implied, including but not
+limited to the implied warranties of merchantability and fitness for
+particular purpose or arising by statute or otherwise in law or from a
+course of dealing or usage in trade, is made by NIMA as to the accuracy and
+functioning of the software.
+
+3. NIMA and its personnel are not required to provide technical support or
+general assistance with respect to the software.
+
+4. Neither NIMA nor its personnel will be liable for any claims, losses, or
+damages arising from or connected with the use of the software. The user
+agrees to hold harmless the United States National Imagery and Mapping
+Agency. The user's sole and exclusive remedy is to stop using the software.
+
+5. NIMA requests that products developed using the software credit the
+source of the software with the following statement, "The product was
+developed using GEOTRANS, a product of the National Imagery and Mapping
+Agency and U.S. Army Engineering Research and Development Center."
+
+6. For any products developed using the software, NIMA requires a disclaimer
+that use of the software does not indicate endorsement or approval of the
+product by the Secretary of Defense or the National Imagery and Mapping
+Agency. Pursuant to the United States Code, 10 U.S.C. Sec. 2797, the name of
+the National Imagery and Mapping Agency, the initials "NIMA", the seal of
+the National Imagery and Mapping Agency, or any colorable imitation thereof
+shall not be used to imply approval, endorsement, or authorization of a
+product without prior written permission from United States Secretary of
+Defense.
+
+*/
+
+#include "datum.h"
+#include "geocentric.h"
+
+
+#ifndef M_PI_2
+	#define M_PI_2 1.57079632679489661923
+#endif // M_PI_2
+#define AD_C 1.0026000
+
+Point3D Geocentric::fromGeodetic(const Coordinates &c, const Datum &datum)
+{
+	const Ellipsoid *e = datum.ellipsoid();
+	double e2 = 2.0 * e->flattening() - e->flattening() * e->flattening();
+
+	double lat = deg2rad(c.lat());
+	double lon = deg2rad(c.lon());
+	double slat = sin(lat);
+	double slat2 = slat * slat;
+	double clat = cos(lat);
+	double Rn = e->radius() / (sqrt(1.0 - e2 * slat2));
+
+	if (lon > M_PI)
+		lon -= (2 * M_PI);
+
+	return Point3D(Rn * clat * cos(lon), Rn * clat * sin(lon),
+	  (Rn * (1 - e2)) * slat);
+}
+
+Coordinates Geocentric::toGeodetic(const Point3D &p, const Datum &datum)
+{
+	const Ellipsoid *e = datum.ellipsoid();
+	double b = e->radius() * (1.0 - e->flattening());
+	double e2 = 2.0 * e->flattening() - e->flattening() * e->flattening();
+	double ep2 = (1.0 / (1.0 - e2)) - 1.0;
+
+	bool pole = false;
+	double lat, lon;
+
+
+	if (p.x() == 0.0) {
+		if (p.y() > 0)
+			lon = M_PI_2;
+		else if (p.y() < 0)
+			lon = -M_PI_2;
+		else {
+			pole = true;
+			lon = 0.0;
+			if (p.z() > 0.0)
+				lat = M_PI_2;
+			else if (p.z() < 0.0)
+				lat = -M_PI_2;
+			else
+				return Coordinates(rad2deg(lon), rad2deg(M_PI_2));
+		}
+	} else
+		lon = atan2(p.y(), p.x());
+
+	double W2 = p.x()*p.x() + p.y()*p.y();
+	double W = sqrt(W2);
+	double T0 = p.z() * AD_C;
+	double S0 = sqrt(T0 * T0 + W2);
+	double Sin_B0 = T0 / S0;
+	double Cos_B0 = W / S0;
+	double Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
+	double T1 = p.z() + b * ep2 * Sin3_B0;
+	double Sum = W - e->radius() * e2 * Cos_B0 * Cos_B0 * Cos_B0;
+	double S1 = sqrt(T1*T1 + Sum * Sum);
+	double Sin_p1 = T1 / S1;
+	double Cos_p1 = Sum / S1;
+
+	if (!pole)
+		lat = atan(Sin_p1 / Cos_p1);
+
+	return Coordinates(rad2deg(lon), rad2deg(lat));
+}

src/map/geocentric.h

@@ -0,0 +1,29 @@
+#ifndef GEOCENTRIC_H
+#define GEOCENTRIC_H
+
+#include <cmath>
+#include "common/coordinates.h"
+
+class Datum;
+
+class Point3D {
+public:
+	Point3D() : _x(NAN), _y(NAN), _z(NAN) {}
+	Point3D(double x, double y, double z) : _x(x), _y(y), _z(z) {}
+
+	double x() const {return _x;}
+	double y() const {return _y;}
+	double z() const {return _z;}
+
+private:
+	double _x;
+	double _y;
+	double _z;
+};
+
+namespace Geocentric {
+	Point3D fromGeodetic(const Coordinates &c, const Datum &datum);
+	Coordinates toGeodetic(const Point3D &p, const Datum &datum);
+}
+
+#endif // GEOCENTRIC_H