mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-03-23 07:15:35 +08:00
99 lines
2.6 KiB
C++
99 lines
2.6 KiB
C++
/* Copyright (C) 2013 VATSIM Community / contributors
|
|
* This Source Code Form is subject to the terms of the Mozilla Public
|
|
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
|
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
*
|
|
* This file incorporates work covered by the following copyright and
|
|
* permission notice:
|
|
* Copyright (c) Charles Karney (2008-2011) <charles@karney.com> and licensed
|
|
* under the MIT/X11 License. For more information, see http://geographiclib.sourceforge.net/
|
|
*/
|
|
|
|
#include "coordinatetransformation.h"
|
|
using namespace BlackMisc::PhysicalQuantities;
|
|
using namespace BlackMisc::Math;
|
|
|
|
namespace BlackMisc
|
|
{
|
|
namespace Geo
|
|
{
|
|
|
|
/*
|
|
* NED to ECEF
|
|
*/
|
|
CCoordinateEcef CCoordinateTransformation::toEcef(const CCoordinateNed &ned)
|
|
{
|
|
CLatitude lat = ned.referencePosition().latitude();
|
|
CLongitude lon = ned.referencePosition().longitude();
|
|
double angleRad = - (lat.value(CAngleUnit::rad())) - BlackMisc::Math::PI / 2;
|
|
|
|
CMatrix3x3 dcm1;
|
|
CMatrix3x3 dcm2;
|
|
CMatrix3x3 dcm3;
|
|
CMatrix3x3 dcm;
|
|
CMatrix3x3 invDcm;
|
|
dcm1.setToIdentity();
|
|
dcm2.setZero();
|
|
dcm3.setZero();
|
|
|
|
dcm2(0, 0) = cos(angleRad);
|
|
dcm2(0, 2) = -sin(angleRad);
|
|
dcm2(1, 1) = 1;
|
|
dcm2(2, 0) = sin(angleRad);
|
|
dcm2(2, 2) = cos(angleRad);
|
|
|
|
angleRad = lon.value(CAngleUnit::rad());
|
|
dcm3(0, 0) = cos(angleRad);
|
|
dcm3(0, 1) = sin(angleRad);
|
|
dcm3(1, 0) = -sin(angleRad);
|
|
dcm3(1, 1) = cos(angleRad);
|
|
dcm3(2, 2) = 1;
|
|
|
|
dcm = dcm1 * dcm2 * dcm3;
|
|
|
|
invDcm.setZero();
|
|
invDcm = dcm.inverse();
|
|
|
|
CVector3D tempResult = invDcm * ned.toMathVector(); // to generic vector
|
|
CCoordinateEcef result(tempResult);
|
|
return result;
|
|
}
|
|
|
|
/*
|
|
* Convert to NED
|
|
*/
|
|
CCoordinateNed toNed(const CCoordinateEcef &ecef, const CCoordinateGeodetic &geo) {
|
|
|
|
CLatitude lat = geo.latitude();
|
|
CLongitude lon = geo.longitude();
|
|
double angleRad = - (lat.value(CAngleUnit::rad())) - BlackMisc::Math::PI / 2;
|
|
|
|
CMatrix3x3 dcm1;
|
|
CMatrix3x3 dcm2(0.0);
|
|
CMatrix3x3 dcm3(0.0);
|
|
CMatrix3x3 dcm(0.0);
|
|
dcm1.setToIdentity();
|
|
|
|
dcm2(0,0) = cos( angleRad );
|
|
dcm2(0,2) = -sin( angleRad );
|
|
dcm2(1,1) = 1;
|
|
dcm2(2,0) = sin( angleRad );
|
|
dcm2(2,2) = cos( angleRad );
|
|
|
|
angleRad = lon.value(CAngleUnit::rad());
|
|
dcm3(0,0) = cos(angleRad );
|
|
dcm3(0,1) = sin(angleRad );
|
|
dcm3(1,0) = -sin(angleRad );
|
|
dcm3(1,1) = cos(angleRad );
|
|
dcm3(2,2) = 1;
|
|
|
|
dcm = dcm1 * dcm2 * dcm3;
|
|
|
|
CVector3D tempResult = dcm * ecef.toMathVector(); // to generic vector
|
|
CCoordinateNed result(geo, tempResult);
|
|
return result;
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace
|