diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 74e6e2d72896f5959d4f1c150655cd37030e7697..c7dd80f4d8527309cad3fc4edff23d0aace28a47 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -487,8 +487,9 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { HEADERS += \ src/AnalyzeView/ExifParser.h \ - src/AnalyzeView/ULogParser.h \ + src/AnalyzeView/LogDownloadController.h \ src/AnalyzeView/PX4LogParser.h \ + src/AnalyzeView/ULogParser.h \ src/Audio/AudioOutput.h \ src/Camera/QGCCameraControl.h \ src/Camera/QGCCameraIO.h \ @@ -553,6 +554,7 @@ HEADERS += \ src/QGCToolbox.h \ src/QmlControls/AppMessages.h \ src/QmlControls/CoordinateVector.h \ + src/QmlControls/EditPositionDialogController.h \ src/QmlControls/MavlinkQmlSingleton.h \ src/QmlControls/ParameterEditorController.h \ src/QmlControls/QGCFileDialogController.h \ @@ -586,7 +588,7 @@ HEADERS += \ src/uas/UAS.h \ src/uas/UASInterface.h \ src/uas/UASMessageHandler.h \ - src/AnalyzeView/LogDownloadController.h \ + src/UTM.h \ AndroidBuild { HEADERS += \ @@ -679,8 +681,9 @@ AndroidBuild { SOURCES += \ src/AnalyzeView/ExifParser.cc \ - src/AnalyzeView/ULogParser.cc \ + src/AnalyzeView/LogDownloadController.cc \ src/AnalyzeView/PX4LogParser.cc \ + src/AnalyzeView/ULogParser.cc \ src/Audio/AudioOutput.cc \ src/Camera/QGCCameraControl.cc \ src/Camera/QGCCameraIO.cc \ @@ -741,6 +744,7 @@ SOURCES += \ src/QGCToolbox.cc \ src/QmlControls/AppMessages.cc \ src/QmlControls/CoordinateVector.cc \ + src/QmlControls/EditPositionDialogController.cc \ src/QmlControls/ParameterEditorController.cc \ src/QmlControls/QGCFileDialogController.cc \ src/QmlControls/QGCImageProvider.cc \ @@ -772,7 +776,7 @@ SOURCES += \ src/main.cc \ src/uas/UAS.cc \ src/uas/UASMessageHandler.cc \ - src/AnalyzeView/LogDownloadController.cc \ + src/UTM.cpp \ DebugBuild { SOURCES += \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index b6a348fe06eeda4a1570c4c5e4ad772912dc9815..ba2116c9da029a912a99824f590041b6c224ac4e 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -48,6 +48,7 @@ src/PlanView/CameraSection.qml src/QmlControls/ClickableColor.qml src/QmlControls/DropButton.qml + src/QmlControls/EditPositionDialog.qml src/QmlControls/ExclusiveGroupItem.qml src/QmlControls/FactSliderPanel.qml src/QmlControls/FileButton.qml @@ -211,6 +212,7 @@ src/MissionManager/CameraSpec.FactMetaData.json src/MissionManager/SpeedSection.FactMetaData.json src/MissionManager/MissionSettings.FactMetaData.json + src/QmlControls/EditPositionDialog.FactMetaData.json src/Vehicle/VehicleFact.json src/Vehicle/BatteryFact.json src/Vehicle/GPSFact.json diff --git a/src/MissionManager/QGCMapPolygonVisuals.qml b/src/MissionManager/QGCMapPolygonVisuals.qml index 0c5300f523d4371ecfce839e154e1e6e2975724a..733a8202535a665bafd3eba293ebccc3f3318c95 100644 --- a/src/MissionManager/QGCMapPolygonVisuals.qml +++ b/src/MissionManager/QGCMapPolygonVisuals.qml @@ -11,6 +11,7 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtLocation 5.3 import QtPositioning 5.3 +import QtQuick.Dialogs 1.2 import QGroundControl 1.0 import QGroundControl.ScreenTools 1.0 @@ -378,6 +379,15 @@ Item { } } + Component { + id: editPositionDialog + + EditPositionDialog { + coordinate: mapPolygon.center + onCoordinateChanged: mapPolygon.center = coordinate + } + } + Component { id: centerDragAreaComponent @@ -412,6 +422,12 @@ Item { onTriggered: radiusDialog.visible = true } + MenuItem { + text: qsTr("Edit position..." ) + enabled: _circle + onTriggered: qgcView.showDialog(editPositionDialog, qsTr("Edit Position"), qgcView.showDialogDefaultWidth, StandardButton.Cancel) + } + MenuItem { text: qsTr("Load KML...") onTriggered: kmlLoadDialog.openForLoad() diff --git a/src/PlanView/MissionItemEditor.qml b/src/PlanView/MissionItemEditor.qml index 1fd1167f51a9f8c551c450ac03b6ec4b4eb5cba7..dd643588f178fbdf9122e36d73082e24af2d6b52 100644 --- a/src/PlanView/MissionItemEditor.qml +++ b/src/PlanView/MissionItemEditor.qml @@ -62,6 +62,15 @@ Rectangle { } } + Component { + id: editPositionDialog + + EditPositionDialog { + coordinate: missionItem.coordinate + onCoordinateChanged: missionItem.coordinate = coordinate + } + } + QGCLabel { id: label anchors.verticalCenter: commandPicker.verticalCenter @@ -136,6 +145,12 @@ Rectangle { visible: !_waypointsOnlyMode } + MenuItem { + text: qsTr("Edit position...") + visible: missionItem.specifiesCoordinate + onTriggered: qgcView.showDialog(editPositionDialog, qsTr("Edit Position"), qgcView.showDialogDefaultWidth, StandardButton.Cancel) + } + MenuSeparator { visible: missionItem.isSimpleItem && !_waypointsOnlyMode } diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 724a8b54b0bf35bbc89a47c69b178b6829cdb36f..a0e7d219e1fedcdae5bf4a2e9171abd2ab24272f 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -83,6 +83,7 @@ #include "QGCCameraManager.h" #include "CameraCalc.h" #include "VisualMissionItem.h" +#include "EditPositionDialogController.h" #ifndef NO_SERIAL_LINK #include "SerialLink.h" @@ -379,6 +380,7 @@ void QGCApplication::_initCommon(void) qmlRegisterType ("QGroundControl.Controllers", 1, 0, "JoystickConfigController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "LogDownloadController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "SyslinkComponentController"); + qmlRegisterType ("QGroundControl.Controllers", 1, 0, "EditPositionDialogController"); #ifndef __mobile__ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ViewWidgetController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "CustomCommandWidgetController"); diff --git a/src/QGCGeo.cc b/src/QGCGeo.cc index 16eca3665144c9b5a80504f0223db6fee6d14070..bf3f7252bc5d699cd0ac68a07533763e97157b60 100644 --- a/src/QGCGeo.cc +++ b/src/QGCGeo.cc @@ -7,11 +7,13 @@ * ****************************************************************************/ +#include #include #include #include "QGCGeo.h" +#include "UTM.h" // These defines are private #define M_DEG_TO_RAD (M_PI / 180.0) @@ -81,3 +83,16 @@ void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCo coord->setAltitude(-z + origin.altitude()); } +int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northing) +{ + return LatLonToUTMXY(coord.latitude(), coord.longitude(), -1 /* zone */, easting, northing); +} + +void convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate& coord) +{ + double latRadians, lonRadians; + + UTMXYToLatLon (easting, northing, zone, southhemi, latRadians, lonRadians); + coord.setLatitude(RadToDeg(latRadians)); + coord.setLongitude(RadToDeg(lonRadians)); +} diff --git a/src/QGCGeo.h b/src/QGCGeo.h index 77e2b3b0b80ded5d56f3cdee9ce09f739359d6ce..18f91ed2cdff24e1d267d18fde9577ece26f6829 100644 --- a/src/QGCGeo.h +++ b/src/QGCGeo.h @@ -41,4 +41,44 @@ void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, dou */ void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCoordinate *coord); +// LatLonToUTMXY +// Converts a latitude/longitude pair to x and y coordinates in the +// Universal Transverse Mercator projection. +// +// Inputs: +// lat - Latitude of the point, in radians. +// lon - Longitude of the point, in radians. +// zone - UTM zone to be used for calculating values for x and y. +// If zone is less than 1 or greater than 60, the routine +// will determine the appropriate zone from the value of lon. +// +// Outputs: +// x - The x coordinate (easting) of the computed point. (in meters) +// y - The y coordinate (northing) of the computed point. (in meters) +// +// Returns: +// The UTM zone used for calculating the values of x and y. +int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northing); + +// UTMXYToLatLon +// +// Converts x and y coordinates in the Universal Transverse Mercator// The UTM zone parameter should be in the range [1,60]. + +// projection to a latitude/longitude pair. +// +// Inputs: +// x - The easting of the point, in meters. +// y - The northing of the point, in meters. +// zone - The UTM zone in which the point lies. +// southhemi - True if the point is in the southern hemisphere; +// false otherwise. +// +// Outputs: +// lat - The latitude of the point, in radians. +// lon - The longitude of the point, in radians. +// +// Returns: +// The function does not return a value. +void convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate& coord); + #endif // QGCGEO_H diff --git a/src/QmlControls/EditPositionDialog.FactMetaData.json b/src/QmlControls/EditPositionDialog.FactMetaData.json new file mode 100644 index 0000000000000000000000000000000000000000..84553deec295cd58c3128c7d9971fcdd3de52102 --- /dev/null +++ b/src/QmlControls/EditPositionDialog.FactMetaData.json @@ -0,0 +1,44 @@ +[ +{ + "name": "Latitude", + "shortDescription": "Latitude of item position", + "type": "double", + "min": -90.0, + "max": 90.0, + "decimalPlaces": 7 +}, +{ + "name": "Longitude", + "shortDescription": "Longitude of item position", + "type": "double", + "min": -180.0, + "max": 180.0, + "decimalPlaces": 7 +}, +{ + "name": "Easting", + "shortDescription": "Easting of item position", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "Northing", + "shortDescription": "Northing of item position", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "Zone", + "shortDescription": "UTM zone", + "type": "uint8", + "min": 1, + "max": 60 +}, +{ + "name": "Hemisphere", + "shortDescription": "Hemisphere for position", + "type": "uint8", + "enumStrings": "North,South", + "enumValues": "0,1" +} +] diff --git a/src/QmlControls/EditPositionDialog.qml b/src/QmlControls/EditPositionDialog.qml new file mode 100644 index 0000000000000000000000000000000000000000..bd20d0b51578d86755150a15dac92ed2449f106b --- /dev/null +++ b/src/QmlControls/EditPositionDialog.qml @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQuick.Layouts 1.2 + +import QGroundControl 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controllers 1.0 + +QGCViewDialog { + property alias coordinate: controller.coordinate + + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 + + EditPositionDialogController { + id: controller + + Component.onCompleted: initValues() + } + + QGCFlickable { + anchors.fill: parent + contentHeight: column.height + + Column { + id: column + anchors.left: parent.left + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelHeight + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columnSpacing: _margin + rowSpacing: _margin + columns: 2 + + QGCLabel { text: qsTr("Latitude") } + FactTextField { + fact: controller.latitude + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Longitude") } + FactTextField { + fact: controller.longitude + Layout.fillWidth: true + } + } + + QGCButton { + anchors.right: parent.right + text: qsTr("Set Geographic") + + onClicked: { + controller.setFromGeo() + reject() + } + } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columnSpacing: _margin + rowSpacing: _margin + columns: 2 + + QGCLabel { text: qsTr("Zone") } + FactTextField { + fact: controller.zone + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Hemisphere") } + FactComboBox { + fact: controller.hemisphere + indexModel: false + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Easting") } + FactTextField { + fact: controller.easting + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Northing") } + FactTextField { + fact: controller.northing + Layout.fillWidth: true + } + } + + QGCButton { + anchors.right: parent.right + text: qsTr("Set UTM") + + onClicked: { + controller.setFromUTM() + reject() + } + } + } // Column + } // QGCFlickable +} // QGCViewDialog diff --git a/src/QmlControls/EditPositionDialogController.cc b/src/QmlControls/EditPositionDialogController.cc new file mode 100644 index 0000000000000000000000000000000000000000..1ad11b5c9f05ab431684173c45011b69e7278943 --- /dev/null +++ b/src/QmlControls/EditPositionDialogController.cc @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "EditPositionDialogController.h" +#include "QGCGeo.h" + +const char* EditPositionDialogController::_latitudeFactName = "Latitude"; +const char* EditPositionDialogController::_longitudeFactName = "Longitude"; +const char* EditPositionDialogController::_zoneFactName = "Zone"; +const char* EditPositionDialogController::_hemisphereFactName = "Hemisphere"; +const char* EditPositionDialogController::_eastingFactName = "Easting"; +const char* EditPositionDialogController::_northingFactName = "Northing"; + +QMap EditPositionDialogController::_metaDataMap; + +EditPositionDialogController::EditPositionDialogController(void) + : _latitudeFact (0, _latitudeFactName, FactMetaData::valueTypeDouble) + , _longitudeFact (0, _longitudeFactName, FactMetaData::valueTypeDouble) + , _zoneFact (0, _zoneFactName, FactMetaData::valueTypeUint8) + , _hemisphereFact (0, _hemisphereFactName, FactMetaData::valueTypeUint8) + , _eastingFact (0, _eastingFactName, FactMetaData::valueTypeDouble) + , _northingFact (0, _northingFactName, FactMetaData::valueTypeDouble) +{ + if (_metaDataMap.isEmpty()) { + _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/EditPositionDialog.FactMetaData.json"), NULL /* QObject parent */); + } + + _latitudeFact.setMetaData (_metaDataMap[_latitudeFactName]); + _longitudeFact.setMetaData (_metaDataMap[_longitudeFactName]); + _zoneFact.setMetaData (_metaDataMap[_zoneFactName]); + _hemisphereFact.setMetaData (_metaDataMap[_hemisphereFactName]); + _eastingFact.setMetaData (_metaDataMap[_eastingFactName]); + _northingFact.setMetaData (_metaDataMap[_northingFactName]); +} + +void EditPositionDialogController::setCoordinate(QGeoCoordinate coordinate) +{ + if (coordinate != _coordinate) { + _coordinate = coordinate; + emit coordinateChanged(coordinate); + } +} + +void EditPositionDialogController::initValues(void) +{ + _latitudeFact.setRawValue(_coordinate.latitude()); + _longitudeFact.setRawValue(_coordinate.longitude()); + + double easting, northing; + int zone = convertGeoToUTM(_coordinate, easting, northing); + _zoneFact.setRawValue(zone); + _hemisphereFact.setRawValue(_coordinate.latitude() < 0); + _eastingFact.setRawValue(easting); + _northingFact.setRawValue(northing); +} + +void EditPositionDialogController::setFromGeo(void) +{ + _coordinate.setLatitude(_latitudeFact.rawValue().toDouble()); + _coordinate.setLongitude(_longitudeFact.rawValue().toDouble()); + emit coordinateChanged(_coordinate); +} + +void EditPositionDialogController::setFromUTM(void) +{ + qDebug() << _eastingFact.rawValue().toDouble() << _northingFact.rawValue().toDouble() << _zoneFact.rawValue().toInt() << (_hemisphereFact.rawValue().toInt() == 1); + convertUTMToGeo(_eastingFact.rawValue().toDouble(), _northingFact.rawValue().toDouble(), _zoneFact.rawValue().toInt(), _hemisphereFact.rawValue().toInt() == 1, _coordinate); + qDebug() << _eastingFact.rawValue().toDouble() << _northingFact.rawValue().toDouble() << _zoneFact.rawValue().toInt() << (_hemisphereFact.rawValue().toInt() == 1) << _coordinate; + emit coordinateChanged(_coordinate); +} diff --git a/src/QmlControls/EditPositionDialogController.h b/src/QmlControls/EditPositionDialogController.h new file mode 100644 index 0000000000000000000000000000000000000000..99ecd44ddcb4ec12c3544d88fa0bcae6eb40080e --- /dev/null +++ b/src/QmlControls/EditPositionDialogController.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include "FactSystem.h" + +class EditPositionDialogController : public QObject +{ + Q_OBJECT + +public: + EditPositionDialogController(void); + + Q_PROPERTY(QGeoCoordinate coordinate READ coordinate WRITE setCoordinate NOTIFY coordinateChanged) + Q_PROPERTY(Fact* latitude READ latitude CONSTANT) + Q_PROPERTY(Fact* longitude READ longitude CONSTANT) + Q_PROPERTY(Fact* zone READ zone CONSTANT) + Q_PROPERTY(Fact* hemisphere READ hemisphere CONSTANT) + Q_PROPERTY(Fact* easting READ easting CONSTANT) + Q_PROPERTY(Fact* northing READ northing CONSTANT) + + QGeoCoordinate coordinate(void) const { return _coordinate; } + Fact* latitude (void) { return &_latitudeFact; } + Fact* longitude (void) { return &_longitudeFact; } + Fact* zone (void) { return &_zoneFact; } + Fact* hemisphere(void) { return &_hemisphereFact; } + Fact* easting (void) { return &_eastingFact; } + Fact* northing (void) { return &_northingFact; } + + void setCoordinate(QGeoCoordinate coordinate); + + Q_INVOKABLE void initValues(void); + Q_INVOKABLE void setFromGeo(void); + Q_INVOKABLE void setFromUTM(void); + +signals: + void coordinateChanged(QGeoCoordinate coordinate); + +private: + static QMap _metaDataMap; + + QGeoCoordinate _coordinate; + + Fact _latitudeFact; + Fact _longitudeFact; + Fact _zoneFact; + Fact _hemisphereFact; + Fact _eastingFact; + Fact _northingFact; + + static const char* _latitudeFactName; + static const char* _longitudeFactName; + static const char* _zoneFactName; + static const char* _hemisphereFactName; + static const char* _eastingFactName; + static const char* _northingFactName; +}; diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index 2701dbeef8d750803e4816bc15675bb51fa1f692..82ffdc70fad541820d0fdb1e6095de125cb289b0 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -7,6 +7,7 @@ CameraSection 1.0 CameraSection.qml ClickableColor 1.0 ClickableColor.qml DropButton 1.0 DropButton.qml DropPanel 1.0 DropPanel.qml +EditPositionDialog 1.0 EditPositionDialog.qml ExclusiveGroupItem 1.0 ExclusiveGroupItem.qml FactSliderPanel 1.0 FactSliderPanel.qml FileButton 1.0 FileButton.qml diff --git a/src/UTM.cpp b/src/UTM.cpp new file mode 100644 index 0000000000000000000000000000000000000000..64e597095a66b59467afb66b55453b873e88e117 --- /dev/null +++ b/src/UTM.cpp @@ -0,0 +1,421 @@ +// UTM.c + +// Original Javascript by Chuck Taylor +// Port to C++ by Alex Hajnal +// +// *** THIS CODE USES 32-BIT FLOATS BY DEFAULT *** +// *** For 64-bit double-precision edit UTM.h: undefine FLOAT_32 and define FLOAT_64 +// +// This is a simple port of the code on the Geographic/UTM Coordinate Converter (1) page from Javascript to C++. +// Using this you can easily convert between UTM and WGS84 (latitude and longitude). +// Accuracy seems to be around 50cm (I suspect rounding errors are limiting precision). +// This code is provided as-is and has been minimally tested; enjoy but use at your own risk! +// The license for UTM.cpp and UTM.h is the same as the original Javascript: +// "The C++ source code in UTM.cpp and UTM.h may be copied and reused without restriction." +// +// 1) http://home.hiwaay.net/~taylorc/toolbox/geography/geoutm.html + +#include "UTM.h" + +// DegToRad +// Converts degrees to radians. +FLOAT DegToRad(FLOAT deg) { + return (deg / 180.0 * pi); +} + + +// RadToDeg +// Converts radians to degrees. +FLOAT RadToDeg(FLOAT rad) { + return (rad / pi * 180.0); +} + +// ArcLengthOfMeridian +// Computes the ellipsoidal distance from the equator to a point at a +// given latitude. +// +// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J., +// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994. +// +// Inputs: +// phi - Latitude of the point, in radians. +// +// Globals: +// sm_a - Ellipsoid model major axis. +// sm_b - Ellipsoid model minor axis. +// +// Returns: +// The ellipsoidal distance of the point from the equator, in meters. +FLOAT ArcLengthOfMeridian (FLOAT phi) { + FLOAT alpha, beta, gamma, delta, epsilon, n; + FLOAT result; + + /* Precalculate n */ + n = (sm_a - sm_b) / (sm_a + sm_b); + + /* Precalculate alpha */ + alpha = ((sm_a + sm_b) / 2.0) + * (1.0 + (POW(n, 2.0) / 4.0) + (POW(n, 4.0) / 64.0)); + + /* Precalculate beta */ + beta = (-3.0 * n / 2.0) + (9.0 * POW(n, 3.0) / 16.0) + + (-3.0 * POW(n, 5.0) / 32.0); + + /* Precalculate gamma */ + gamma = (15.0 * POW(n, 2.0) / 16.0) + + (-15.0 * POW(n, 4.0) / 32.0); + + /* Precalculate delta */ + delta = (-35.0 * POW(n, 3.0) / 48.0) + + (105.0 * POW(n, 5.0) / 256.0); + + /* Precalculate epsilon */ + epsilon = (315.0 * POW(n, 4.0) / 512.0); + + /* Now calculate the sum of the series and return */ + result = alpha + * (phi + (beta * SIN(2.0 * phi)) + + (gamma * SIN(4.0 * phi)) + + (delta * SIN(6.0 * phi)) + + (epsilon * SIN(8.0 * phi))); + + return result; +} + + + +// UTMCentralMeridian +// Determines the central meridian for the given UTM zone. +// +// Inputs: +// zone - An integer value designating the UTM zone, range [1,60]. +// +// Returns: +// The central meridian for the given UTM zone, in radians +// Range of the central meridian is the radian equivalent of [-177,+177]. +FLOAT UTMCentralMeridian(int zone) { + FLOAT cmeridian; + cmeridian = DegToRad(-183.0 + ((FLOAT)zone * 6.0)); + + return cmeridian; +} + + + +// FootpointLatitude +// +// Computes the footpoint latitude for use in converting transverse +// Mercator coordinates to ellipsoidal coordinates. +// +// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J., +// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994. +// +// Inputs: +// y - The UTM northing coordinate, in meters. +// +// Returns: +// The footpoint latitude, in radians. +FLOAT FootpointLatitude(FLOAT y) { + FLOAT y_, alpha_, beta_, gamma_, delta_, epsilon_, n; + FLOAT result; + + /* Precalculate n (Eq. 10.18) */ + n = (sm_a - sm_b) / (sm_a + sm_b); + + /* Precalculate alpha_ (Eq. 10.22) */ + /* (Same as alpha in Eq. 10.17) */ + alpha_ = ((sm_a + sm_b) / 2.0) + * (1 + (POW(n, 2.0) / 4) + (POW(n, 4.0) / 64)); + + /* Precalculate y_ (Eq. 10.23) */ + y_ = y / alpha_; + + /* Precalculate beta_ (Eq. 10.22) */ + beta_ = (3.0 * n / 2.0) + (-27.0 * POW(n, 3.0) / 32.0) + + (269.0 * POW(n, 5.0) / 512.0); + + /* Precalculate gamma_ (Eq. 10.22) */ + gamma_ = (21.0 * POW(n, 2.0) / 16.0) + + (-55.0 * POW(n, 4.0) / 32.0); + + /* Precalculate delta_ (Eq. 10.22) */ + delta_ = (151.0 * POW(n, 3.0) / 96.0) + + (-417.0 * POW(n, 5.0) / 128.0); + + /* Precalculate epsilon_ (Eq. 10.22) */ + epsilon_ = (1097.0 * POW(n, 4.0) / 512.0); + + /* Now calculate the sum of the series (Eq. 10.21) */ + result = y_ + (beta_ * SIN(2.0 * y_)) + + (gamma_ * SIN(4.0 * y_)) + + (delta_ * SIN(6.0 * y_)) + + (epsilon_ * SIN(8.0 * y_)); + + return result; +} + + + +// MapLatLonToXY +// Converts a latitude/longitude pair to x and y coordinates in the +// Transverse Mercator projection. Note that Transverse Mercator is not +// the same as UTM; a scale factor is required to convert between them. +// +// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J., +// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994. +// +// Inputs: +// phi - Latitude of the point, in radians. +// lambda - Longitude of the point, in radians. +// lambda0 - Longitude of the central meridian to be used, in radians. +// +// Outputs: +// x - The x coordinate of the computed point. +// y - The y coordinate of the computed point. +// +// Returns: +// The function does not return a value. +void MapLatLonToXY (FLOAT phi, FLOAT lambda, FLOAT lambda0, FLOAT &x, FLOAT &y) { + FLOAT N, nu2, ep2, t, t2, l; + FLOAT l3coef, l4coef, l5coef, l6coef, l7coef, l8coef; + //FLOAT tmp; // Unused + + /* Precalculate ep2 */ + ep2 = (POW(sm_a, 2.0) - POW(sm_b, 2.0)) / POW(sm_b, 2.0); + + /* Precalculate nu2 */ + nu2 = ep2 * POW(COS(phi), 2.0); + + /* Precalculate N */ + N = POW(sm_a, 2.0) / (sm_b * SQRT(1 + nu2)); + + /* Precalculate t */ + t = TAN(phi); + t2 = t * t; + //tmp = (t2 * t2 * t2) - POW(t, 6.0); // Unused + + /* Precalculate l */ + l = lambda - lambda0; + + /* Precalculate coefficients for l**n in the equations below + so a normal human being can read the expressions for easting + and northing + -- l**1 and l**2 have coefficients of 1.0 */ + l3coef = 1.0 - t2 + nu2; + + l4coef = 5.0 - t2 + 9 * nu2 + 4.0 * (nu2 * nu2); + + l5coef = 5.0 - 18.0 * t2 + (t2 * t2) + 14.0 * nu2 + - 58.0 * t2 * nu2; + + l6coef = 61.0 - 58.0 * t2 + (t2 * t2) + 270.0 * nu2 + - 330.0 * t2 * nu2; + + l7coef = 61.0 - 479.0 * t2 + 179.0 * (t2 * t2) - (t2 * t2 * t2); + + l8coef = 1385.0 - 3111.0 * t2 + 543.0 * (t2 * t2) - (t2 * t2 * t2); + + /* Calculate easting (x) */ + x = N * COS(phi) * l + + (N / 6.0 * POW(COS(phi), 3.0) * l3coef * POW(l, 3.0)) + + (N / 120.0 * POW(COS(phi), 5.0) * l5coef * POW(l, 5.0)) + + (N / 5040.0 * POW(COS(phi), 7.0) * l7coef * POW(l, 7.0)); + + /* Calculate northing (y) */ + y = ArcLengthOfMeridian (phi) + + (t / 2.0 * N * POW(COS(phi), 2.0) * POW(l, 2.0)) + + (t / 24.0 * N * POW(COS(phi), 4.0) * l4coef * POW(l, 4.0)) + + (t / 720.0 * N * POW(COS(phi), 6.0) * l6coef * POW(l, 6.0)) + + (t / 40320.0 * N * POW(COS(phi), 8.0) * l8coef * POW(l, 8.0)); + + return; +} + + + +// MapXYToLatLon +// Converts x and y coordinates in the Transverse Mercator projection to +// a latitude/longitude pair. Note that Transverse Mercator is not +// the same as UTM; a scale factor is required to convert between them. +// +// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J., +// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994. +// +// Inputs: +// x - The easting of the point, in meters. +// y - The northing of the point, in meters. +// lambda0 - Longitude of the central meridian to be used, in radians. +// +// Outputs: +// phi - Latitude in radians. +// lambda - Longitude in radians. +// +// Returns: +// The function does not return a value. +// +// Remarks: +// The local variables Nf, nuf2, tf, and tf2 serve the same purpose as +// N, nu2, t, and t2 in MapLatLonToXY, but they are computed with respect +// to the footpoint latitude phif. +// +// x1frac, x2frac, x2poly, x3poly, etc. are to enhance readability and +// to optimize computations. +void MapXYToLatLon (FLOAT x, FLOAT y, FLOAT lambda0, FLOAT& phi, FLOAT& lambda) +{ + FLOAT phif, Nf, Nfpow, nuf2, ep2, tf, tf2, tf4, cf; + FLOAT x1frac, x2frac, x3frac, x4frac, x5frac, x6frac, x7frac, x8frac; + FLOAT x2poly, x3poly, x4poly, x5poly, x6poly, x7poly, x8poly; + + /* Get the value of phif, the footpoint latitude. */ + phif = FootpointLatitude (y); + + /* Precalculate ep2 */ + ep2 = (POW(sm_a, 2.0) - POW(sm_b, 2.0)) + / POW(sm_b, 2.0); + + /* Precalculate cos (phif) */ + cf = COS(phif); + + /* Precalculate nuf2 */ + nuf2 = ep2 * POW(cf, 2.0); + + /* Precalculate Nf and initialize Nfpow */ + Nf = POW(sm_a, 2.0) / (sm_b * SQRT(1 + nuf2)); + Nfpow = Nf; + + /* Precalculate tf */ + tf = TAN(phif); + tf2 = tf * tf; + tf4 = tf2 * tf2; + + /* Precalculate fractional coefficients for x**n in the equations + below to simplify the expressions for latitude and longitude. */ + x1frac = 1.0 / (Nfpow * cf); + + Nfpow *= Nf; /* now equals Nf**2) */ + x2frac = tf / (2.0 * Nfpow); + + Nfpow *= Nf; /* now equals Nf**3) */ + x3frac = 1.0 / (6.0 * Nfpow * cf); + + Nfpow *= Nf; /* now equals Nf**4) */ + x4frac = tf / (24.0 * Nfpow); + + Nfpow *= Nf; /* now equals Nf**5) */ + x5frac = 1.0 / (120.0 * Nfpow * cf); + + Nfpow *= Nf; /* now equals Nf**6) */ + x6frac = tf / (720.0 * Nfpow); + + Nfpow *= Nf; /* now equals Nf**7) */ + x7frac = 1.0 / (5040.0 * Nfpow * cf); + + Nfpow *= Nf; /* now equals Nf**8) */ + x8frac = tf / (40320.0 * Nfpow); + + /* Precalculate polynomial coefficients for x**n. + -- x**1 does not have a polynomial coefficient. */ + x2poly = -1.0 - nuf2; + + x3poly = -1.0 - 2 * tf2 - nuf2; + + x4poly = 5.0 + 3.0 * tf2 + 6.0 * nuf2 - 6.0 * tf2 * nuf2 + - 3.0 * (nuf2 *nuf2) - 9.0 * tf2 * (nuf2 * nuf2); + + x5poly = 5.0 + 28.0 * tf2 + 24.0 * tf4 + 6.0 * nuf2 + 8.0 * tf2 * nuf2; + + x6poly = -61.0 - 90.0 * tf2 - 45.0 * tf4 - 107.0 * nuf2 + + 162.0 * tf2 * nuf2; + + x7poly = -61.0 - 662.0 * tf2 - 1320.0 * tf4 - 720.0 * (tf4 * tf2); + + x8poly = 1385.0 + 3633.0 * tf2 + 4095.0 * tf4 + 1575 * (tf4 * tf2); + + /* Calculate latitude */ + phi = phif + x2frac * x2poly * (x * x) + + x4frac * x4poly * POW(x, 4.0) + + x6frac * x6poly * POW(x, 6.0) + + x8frac * x8poly * POW(x, 8.0); + + /* Calculate longitude */ + lambda = lambda0 + x1frac * x + + x3frac * x3poly * POW(x, 3.0) + + x5frac * x5poly * POW(x, 5.0) + + x7frac * x7poly * POW(x, 7.0); + + return; +} + + + + +// LatLonToUTMXY +// Converts a latitude/longitude pair to x and y coordinates in the +// Universal Transverse Mercator projection. +// +// Inputs: +// lat - Latitude of the point, in radians. +// lon - Longitude of the point, in radians. +// zone - UTM zone to be used for calculating values for x and y. +// If zone is less than 1 or greater than 60, the routine +// will determine the appropriate zone from the value of lon. +// +// Outputs: +// x - The x coordinate (easting) of the computed point. (in meters) +// y - The y coordinate (northing) of the computed point. (in meters) +// +// Returns: +// The UTM zone used for calculating the values of x and y. +int LatLonToUTMXY (FLOAT lat, FLOAT lon, int zone, FLOAT& x, FLOAT& y) { + if ( (zone < 1) || (zone > 60) ) + zone = FLOOR((lon + 180.0) / 6) + 1; + + MapLatLonToXY (DegToRad(lat), DegToRad(lon), UTMCentralMeridian(zone), x, y); + + /* Adjust easting and northing for UTM system. */ + x = x * UTMScaleFactor + 500000.0; + y = y * UTMScaleFactor; + if (y < 0.0) + y = y + 10000000.0; + + return zone; +} + + + +// UTMXYToLatLon +// +// Converts x and y coordinates in the Universal Transverse Mercator +// projection to a latitude/longitude pair. +// +// Inputs: +// x - The easting of the point, in meters. +// y - The northing of the point, in meters. +// zone - The UTM zone in which the point lies. +// southhemi - True if the point is in the southern hemisphere; +// false otherwise. +// +// Outputs: +// lat - The latitude of the point, in radians. +// lon - The longitude of the point, in radians. +// +// Returns: +// The function does not return a value. +void UTMXYToLatLon (FLOAT x, FLOAT y, int zone, bool southhemi, FLOAT& lat, FLOAT& lon) { + FLOAT cmeridian; + + x -= 500000.0; + x /= UTMScaleFactor; + + /* If in southern hemisphere, adjust y accordingly. */ + if (southhemi) + y -= 10000000.0; + + y /= UTMScaleFactor; + + cmeridian = UTMCentralMeridian (zone); + MapXYToLatLon (x, y, cmeridian, lat, lon); + + return; +} + diff --git a/src/UTM.h b/src/UTM.h new file mode 100644 index 0000000000000000000000000000000000000000..b7d9d9a360042513aa3ae4e77a58f46e206bbde3 --- /dev/null +++ b/src/UTM.h @@ -0,0 +1,206 @@ +// UTM.h + +// Original Javascript by Chuck Taylor +// Port to C++ by Alex Hajnal +// +// *** THIS CODE USES 32-BIT FLOATS BY DEFAULT *** +// *** For 64-bit double-precision edit this file: undefine FLOAT_32 and define FLOAT_64 (see below) +// +// This is a simple port of the code on the Geographic/UTM Coordinate Converter (1) page from Javascript to C++. +// Using this you can easily convert between UTM and WGS84 (latitude and longitude). +// Accuracy seems to be around 50cm (I suspect rounding errors are limiting precision). +// This code is provided as-is and has been minimally tested; enjoy but use at your own risk! +// The license for UTM.cpp and UTM.h is the same as the original Javascript: +// "The C++ source code in UTM.cpp and UTM.h may be copied and reused without restriction." +// +// 1) http://home.hiwaay.net/~taylorc/toolbox/geography/geoutm.html + +#ifndef UTM_H +#define UTM_H + +// Choose floating point precision: + +// 32-bit (for Teensy 3.5/3.6 ARM boards, etc.) +#define FLOAT_64 + +// 64-bit (for desktop/server use) +//#define FLOAT_64 + +#ifdef FLOAT_64 +#define FLOAT double +#define SIN sin +#define COS cos +#define TAN tan +#define POW pow +#define SQRT sqrt +#define FLOOR floor + +#else +#ifdef FLOAT_32 +#define FLOAT float +#define SIN sinf +#define COS cosf +#define TAN tanf +#define POW powf +#define SQRT sqrtf +#define FLOOR floorf + +#endif +#endif + + +#include + +#define pi 3.14159265358979 + +/* Ellipsoid model constants (actual values here are for WGS84) */ +#define sm_a 6378137.0 +#define sm_b 6356752.314 +#define sm_EccSquared 6.69437999013e-03 + +#define UTMScaleFactor 0.9996 + +// DegToRad +// Converts degrees to radians. +FLOAT DegToRad(FLOAT deg); + +// RadToDeg +// Converts radians to degrees. +FLOAT RadToDeg(FLOAT rad); + +// ArcLengthOfMeridian +// Computes the ellipsoidal distance from the equator to a point at a +// given latitude. +// +// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J., +// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994. +// +// Inputs: +// phi - Latitude of the point, in radians. +// +// Globals: +// sm_a - Ellipsoid model major axis. +// sm_b - Ellipsoid model minor axis. +// +// Returns: +// The ellipsoidal distance of the point from the equator, in meters. +FLOAT ArcLengthOfMeridian (FLOAT phi); + +// UTMCentralMeridian +// Determines the central meridian for the given UTM zone. +// +// Inputs: +// zone - An integer value designating the UTM zone, range [1,60]. +// +// Returns: +// The central meridian for the given UTM zone, in radians +// Range of the central meridian is the radian equivalent of [-177,+177]. +FLOAT UTMCentralMeridian(int zone); + +// FootpointLatitude +// +// Computes the footpoint latitude for use in converting transverse +// Mercator coordinates to ellipsoidal coordinates. +// +// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J., +// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994. +// +// Inputs: +// y - The UTM northing coordinate, in meters. +// +// Returns: +// The footpoint latitude, in radians. +FLOAT FootpointLatitude(FLOAT y); + +// MapLatLonToXY +// Converts a latitude/longitude pair to x and y coordinates in the +// Transverse Mercator projection. Note that Transverse Mercator is not +// the same as UTM; a scale factor is required to convert between them. +// +// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J., +// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994. +// +// Inputs: +// phi - Latitude of the point, in radians. +// lambda - Longitude of the point, in radians. +// lambda0 - Longitude of the central meridian to be used, in radians. +// +// Outputs: +// x - The x coordinate of the computed point. +// y - The y coordinate of the computed point. +// +// Returns: +// The function does not return a value. +void MapLatLonToXY (FLOAT phi, FLOAT lambda, FLOAT lambda0, FLOAT &x, FLOAT &y); + +// MapXYToLatLon +// Converts x and y coordinates in the Transverse Mercator projection to +// a latitude/longitude pair. Note that Transverse Mercator is not +// the same as UTM; a scale factor is required to convert between them. +// +// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J., +// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994. +// +// Inputs: +// x - The easting of the point, in meters. +// y - The northing of the point, in meters. +// lambda0 - Longitude of the central meridian to be used, in radians. +// +// Outputs: +// phi - Latitude in radians. +// lambda - Longitude in radians. +// +// Returns: +// The function does not return a value. +// +// Remarks: +// The local variables Nf, nuf2, tf, and tf2 serve the same purpose as +// N, nu2, t, and t2 in MapLatLonToXY, but they are computed with respect +// to the footpoint latitude phif. +// +// x1frac, x2frac, x2poly, x3poly, etc. are to enhance readability and +// to optimize computations. +void MapXYToLatLon (FLOAT x, FLOAT y, FLOAT lambda0, FLOAT& phi, FLOAT& lambda); + +// LatLonToUTMXY +// Converts a latitude/longitude pair to x and y coordinates in the +// Universal Transverse Mercator projection. +// +// Inputs: +// lat - Latitude of the point, in radians. +// lon - Longitude of the point, in radians. +// zone - UTM zone to be used for calculating values for x and y. +// If zone is less than 1 or greater than 60, the routine +// will determine the appropriate zone from the value of lon. +// +// Outputs: +// x - The x coordinate (easting) of the computed point. (in meters) +// y - The y coordinate (northing) of the computed point. (in meters) +// +// Returns: +// The UTM zone used for calculating the values of x and y. +int LatLonToUTMXY (FLOAT lat, FLOAT lon, int zone, FLOAT& x, FLOAT& y); + +// UTMXYToLatLon +// +// Converts x and y coordinates in the Universal Transverse Mercator// The UTM zone parameter should be in the range [1,60]. + +// projection to a latitude/longitude pair. +// +// Inputs: +// x - The easting of the point, in meters. +// y - The northing of the point, in meters. +// zone - The UTM zone in which the point lies. +// southhemi - True if the point is in the southern hemisphere; +// false otherwise. +// +// Outputs: +// lat - The latitude of the point, in radians. +// lon - The longitude of the point, in radians. +// +// Returns: +// The function does not return a value. +void UTMXYToLatLon (FLOAT x, FLOAT y, int zone, bool southhemi, FLOAT& lat, FLOAT& lon); + +#endif +