📄 geopos.cpp
字号:
/* Combat Simulator Project * Copyright 2002, 2003, 2004 Mark Rose <mkrose@users.sourceforge.net> * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *//** * @file GeoPos.cpp * @brief Geodetic coordinate class and conversions. *///#ifdef WIN32
#include "stdafx.h"
//#endif
#include "GeoPos.h"#include "Math.h"#include <cmath>#include <sstream>#include <iomanip>#define max(a,b) (((a) > (b)) ? (a) : (b))
#define min(a,b) (((a) < (b)) ? (a) : (b))
#ifndef PI
#define PI 3.14159265358979
#endif
double toRadians (const double de)
{
return de*PI/180.;
}
double toDegrees (const double ra)
{
return ra/PI*180.;
}
namespace GeoRef { const CReferenceEllipsoid GRS80 (6378137.000, 298.2572221); const CReferenceEllipsoid WGS72 (6378135.000, 298.26); const CReferenceEllipsoid Clarke1866 (6378206.400, 294.9786982);
const CReferenceEllipsoid Clarke1880 (6378249.136, 293.465);
const CReferenceEllipsoid Krassolvsky1990 (6378245.000, 298.3000000);
const CReferenceEllipsoid GJ80 (6378140.000, 298.2570000);
const CReferenceEllipsoid WGS84 (6378137.000, 298.257223563);}////////////////////////////////////////////////////////////////////////////////// Geospacial Library Functions
//////////////////////////////////////////////////////////////////////////
//CReferenceEllipsoid
CReferenceEllipsoid::CReferenceEllipsoid(double semi_major, double flattening_inv)
{
this->setPara(semi_major,flattening_inv);
}
void CReferenceEllipsoid::setPara(double semi_major, double flattening_inv)
{
A = semi_major;
f = 1.0/flattening_inv;
B = A*(1-f);
R = 6371010.0;
// f = (A-B)/A; //flattening
e = sqrt(2*f-f*f); //the first eccentricity
A_B = A/B;
B_A = B/A;
B2_A2 = (B*B)/(A*A);
A2_B2 = 1.0 / B2_A2;
e2 = e*e;
e1 = (A-B)/(A+B);
ep = sqrt(A*A-B*B)/B; //the second eccentricity
ep2 = ep*ep;
// LLA2UTM constants used to calculate the length of arc of a meridian from equator to lat,P67
f8 m0,m2,m4,m6,m8;
m0 = A*(1-e2);
m2 = 3.0/2*e2*m0;
m4 = 5.0/4*e2*m2;
m6 = 7.0/6*e2*m4;
m8 = 9.0/8*e2*m6;
m_0 = m0+m2/2.0+(3.0/8)*m4+(5.0/16)*m6+(35.0/128)*m8;
m_1 = m2/2+m4/2+(15.0/32)*m6+(7.0/16)*m8;
m_2 = m4/8+(3.0/16)*m6+(7.0/32)*m8;
m_3 = m6/32.0+m8/16.0;
// UTM2LLA constants
m_f = 1.0/(A*(1.0 - e2*(0.25 + e2*(0.046875 + e2*0.01953125))));
m_a = e1 * (1.5 - 0.84375*e1*e1);
m_b = e1 * e1 * (1.3125 - 1.71875*e1*e1);
m_c = 1.5729166666667*e1*e1*e1;
}
CReferenceEllipsoid CReferenceEllipsoid::operator =(CReferenceEllipsoid right)
{
this->setPara(right.A,1.0/right.f);
return *this;
}
bool CReferenceEllipsoid::operator == (CReferenceEllipsoid right)
{
return (fabs(this->A - right.A)<1e-10 && fabs(this->f - right.f) < 1e-10);
}
LLA ECEFtoLLA(ECEF const &ecef, CReferenceEllipsoid const &_ref){ double _lon = atan2(ecef.y(), ecef.x()); double _lat, _alt;
double xy = sqrt(ecef.x()*ecef.x()+ecef.y()*ecef.y());
double c = _ref.A*_ref.A / _ref.B;
double p = c*_ref.e2/xy;
double k = 1+_ref.ep2;
double t0 = ecef.z()/xy;
double t=t0;
double t1;
do {
t1=t;
t=t0+p*t1/sqrt(k+t1*t1);
} while(fabs(t1-t)>1e-12);
_lat = atan(t);
_alt = xy/cos(_lat)-c/sqrt(1+_ref.ep2*cos(_lat)*cos(_lat));
return LLA(_lat, _lon, _alt);}ECEF LLAtoECEF(LLA const &lla, CReferenceEllipsoid const &_ref){
//nh=N+H
double n=_ref.A/sqrt(1-_ref.e2*sin(lla._lat)*sin(lla._lat));
double nh=lla.altitude()+n;
return ECEF(nh*cos(lla.latitude())*cos(lla.longitude()),
nh*cos(lla.latitude())*sin(lla.longitude()),
(n*(1-_ref.e2)+lla.altitude())*sin(lla.latitude()));
}ECEF GRIDtoECEF(GRID const &grid, const double k0/* =0.9996 */,
CReferenceEllipsoid const &_ref /* = GeoRef::WGS84 */){ return LLAtoECEF(GRIDtoLLA(grid, k0, _ref), _ref);}GRID ECEFtoGRID(ECEF const &ecef, const double k0/* =0.9996 */,
CReferenceEllipsoid const &_ref /* = GeoRef::WGS84 */){ return LLAtoGRID(ECEFtoLLA(ecef, _ref), k0,_ref);}LLA GRIDtoLLA(GRID const &grid, const double k0/* =0.9996 */,
CReferenceEllipsoid const &_ref /* = GeoRef::WGS84 */){ // central-meridian scale factor double nu, T, T2, S, C, CP, SP, R, D, D2, M; double mu, phi; double x, y; double lon0 = grid.lon0(); x = grid.easting() - 500000.0; //remove 500,000 meter offset for longitude y = grid.northing(); if (grid.NorS() == 'S') { y -= 10000000.0; //remove 10,000,000 meter offset used for southern hemisphere }// double lon0 = toRadians(((utm.zone() - 1) * 6.0 - 180.0 + 3.0)); //+3 puts origin in middle of zone M = y / k0; mu = M * _ref.m_f; phi = mu + _ref.m_a*sin(2.0*mu) + _ref.m_b*sin(4.0*mu) + _ref.m_c*sin(6.0*mu); C = cos(phi); S = sin(phi); T = S/C; nu = _ref.A/sqrt(1.0-_ref.e2*S*S); T2 = T * T; CP = C * C * _ref.ep2; SP = 1.0 - S * S * _ref.e2; R = _ref.A * _ref.B2_A2 / (SP*sqrt(SP)); D = x/(nu*k0); D2 = D*D; double lat, lon; lat = phi - (nu*T/R)*(D2*(0.5 - D2*((120.0+90.0*T2+CP*(300.0-120.0*CP)-270.0*_ref.ep2) +(61.0+90.0*T2+298.0*CP+45.0*T2*T2-252.0*_ref.ep2-3.0*CP*CP)*D2)/720.0)); lon = D * ( 1.0 - D2*( (1.0+2.0*T2+CP)/6.0 - ( 5.0 - CP*(2.0 + 3.0*CP)
+ 8.0*_ref.ep2
+ T2*(28.0 + 24.0*T2)
) * D2/120.0
)
) / C; lon += lon0; return LLA(lat, lon, grid.altitude());}
GRID LLAtoGRID(LLA const &lla,const double k0/* =0.9996 */,
CReferenceEllipsoid const &_ref /* = GeoRef::WGS84 */)
{
// central-meridian scale factor
// double k0 = 0.9996; //k0=1:Gausse projection,=0.9996 GRID
double lon = lla.longitude();
double lat = lla.latitude();
double nu, T, T2, C, CP2, CP4, A, A2, A3, A4, M, S;
char _zone;
double lon0;
GRID::GetZoneAndCM(lon,lat,_zone,lon0);
//Make sure the longitude is between -180.00 .. 179.9
if (lon >= PI) {
int n = (int) (0.5 * lon / PI + 0.5);
lon -= n * 2.0 * PI;
} else
if (lon < -PI) {
int n = (int) (0.5 * lon / PI - 0.5);
lon -= n * 2.0 * PI;
}
S = sin(lat);
C = cos(lat);
T = S/C; //tanB
T2 = T * T; //tanB*tanB
nu = _ref.A/sqrt(1.0-_ref.e2*S*S); //the radius of prime vertical
CP2 = _ref.ep2*C*C; //ep*ep*cosB*cosB
CP4 = CP2*CP2;
A = C*(lon - lon0); //cosB*l
A2 = A * A;
A3 = A2 * A;
A4 = A2 * A2;
// approximation for length of arc of a meridian from equator to lat,P72
M = _ref.m_0*lat
- S*C*((_ref.m_1-_ref.m_2 + _ref.m_3)
+ (2.0*_ref.m_2-(16.0/3)*_ref.m_3)*S*S
+ (16.0/3)*_ref.m_3*S*S*S*S);
double _easting, _northing;
//l<3.5degree, precision 0.001m.
_easting = k0 * nu * ( A + //cosB*l
(1.0 - T2 + CP2) * A3 / 6 + //(1-t*t+cp2) * l^3 * cosB^3/6
(5.0 - 18.0*T2 + T2*T2 + 14*CP2 - 58.0*T2*CP2) * A4 *A / 120.0
) + 500000.0;
_northing = k0 * ( M +
nu * T * ( 0.5*A2 + //1/2 * cosB^2 * l^2
(5.0 - T2 + 9.0*CP2 + 4.0*CP2*CP2)*A4/24.0 +
(61.0 -58.0*T2 + T2*T2 + 270.0*CP2 - 330.0*T2*CP2)*A4*A2/720.0
)
);
if (lat < 0.0) {
_northing += 10000000.0; //10000000 meter offset for southern hemisphere
}
char _designator = GRID::GetDesignator(lat);
return GRID(_easting, _northing, _zone, _designator, lla.altitude());
}//GRID LLAtoGRID(LLA const &lla,char _zone/* =-1 */,const double k0/* =0.9996 */,
// CReferenceEllipsoid const &_ref /* = GeoRef::WGS84 */)
//{
// // central-meridian scale factor
//// double k0 = 0.9996; //k0=1:Gausse projection,=0.9996 GRID
// double lon = lla.longitude();
// double lat = lla.latitude();
// double lon0 = 0.0;
// double nu, T, T2, C, CP2, CP4, A, A2, A3, A4, M, S;
//
// //Make sure the longitude is between -180.00 .. 179.9
// if (lon >= PI) {
// int n = (int) (0.5 * lon / PI + 0.5);
// lon -= n * 2.0 * PI;
// } else
// if (lon < -PI) {
// int n = (int) (0.5 * lon / PI - 0.5);
// lon -= n * 2.0 * PI;
// }
//
// //////////////////////////////////////////////////////////////////////////
// /* UTM Zone 32 has been widened to 9°(at the expense of zone 31) between latitudes 56°and 64°(band V)
// to accommodate southwest Norway. Thus zone 32 it extends westwards to 3°E in the North Sea.
//
// Similarly, between 72° and 84° (band X), zones 33 and 35 have been widened to 12° to accommodate Svalbard.
// To compensate for these 12°wide zones, zones 31 and 37 are widened to 9° and zones 32, 34, and 36 are eliminated.
// Thus the W and E boundaries of zones are 31: 0 - 9 E, 33: 9 - 21 E, 35: 21 - 33 E and 37: 33 - 42 E.
// */
// if (_zone >= 0 && (lat >= 1.2566370614359 && lat < 1.4660765716752 || //56-64
// lat >= 0.9773843811168 && lat < 1.1170107212764) //72-84
// ){
// switch (_zone) {
// case 32:
// lon0 = 0.1308996938996; //7.5degree
// break;
// case 31:
// lon0 = 0.078539816339744;//4.5
// break;
// case 33:
// lon0 = 0.261799387799149;//15
// break;
// case 35:
// lon0 = 0.471238898038469;//27
// break;
// case 37:
// lon0 = 0.654498469497874;//37.5
// break;
// default:
// lon0 = toRadians((int(_zone) - 1)*6.0 - 180.0 + 3.0);
// }
// }
// // special zone for Norway, lat 56 - 64 deg, lon 3 - 12 deg
// if (lat >= 0.9773843811168 && lat < 1.1170107212764 &&
// lon >= 0.0523598775598 && lon < 0.2094395102393) {
// _zone = 32;
// lon0 = 0.1308996938996; //7.5degree
// }
// // special zones for Svalbard, lat 72 - 84
// else if( lat >= 1.2566370614359 && lat < 1.4660765716752) {
// if (lon < 0.0) {
// } else
// if (lon < 0.157079632679490) { // 0 - 9 deg
// _zone = 31;
// lon0 = 0.078539816339744; //4.5degree
// } else
// if (lon < 0.366519142918809) { // 9 - 21 deg
// _zone = 33;
// lon0 = 0.261799387799149; //15degree
// } else
// if (lon < 0.575958653158129) { // 21 - 33 deg
// _zone = 35;
// lon0 = 0.471238898038469; //27degree
// } else
// if (lon < 0.733038285837618) { // 33 - 42 deg
// _zone = 37;
// lon0 = 0.654498469497874; //37.5degree
// }
// }
// if (_zone == -1) {
// _zone = char((lon / PI + 1.0) * 30.0) + 1;
// lon0 = toRadians((int(_zone) - 1)*6.0 - 180.0 + 3.0);
// }
// S = sin(lat);
// C = cos(lat);
// T = S/C; //tanB
// T2 = T * T; //tanB*tanB
// nu = _ref.A/sqrt(1.0-_ref.e2*S*S); //the radius of prime vertical
// CP2 = _ref.ep2*C*C; //ep*ep*cosB*cosB
// CP4 = CP2*CP2;
//
// A = C*(lon - lon0); //cosB*l
// A2 = A * A;
// A3 = A2 * A;
// A4 = A2 * A2;
// // approximation for length of arc of a meridian from equator to lat,P72
// M = _ref.m_0*lat
// - S*C*((_ref.m_1-_ref.m_2 + _ref.m_3)
// + (2.0*_ref.m_2-(16.0/3)*_ref.m_3)*S*S
// + (16.0/3)*_ref.m_3*S*S*S*S);
//
// double _easting, _northing;
// //l<3.5degree, precision 0.001m.
// _easting = k0 * nu * ( A + //cosB*l
// (1.0 - T2 + CP2) * A3 / 6 + //(1-t*t+cp2) * l^3 * cosB^3/6
// (5.0 - 18.0*T2 + T2*T2 + 14*CP2 - 58.0*T2*CP2) * A4 *A / 120.0
// ) + 500000.0;
// _northing = k0 * ( M +
// nu * T * ( 0.5*A2 + //1/2 * cosB^2 * l^2
// (5.0 - T2 + 9.0*CP2 + 4.0*CP2*CP2)*A4/24.0 +
// (61.0 -58.0*T2 + T2*T2 + 270.0*CP2 - 330.0*T2*CP2)*A4*A2/720.0
// )
// );
// if (lat < 0.0) {
// _northing += 10000000.0; //10000000 meter offset for southern hemisphere
// }
// char _designator = GRID::GetDesignator(lat);
// return GRID(_easting, _northing, _zone, _designator, lla.altitude());
//}
//void LLAtoGRID(double lon,double lat,double alt,double* utm,char _zone,const double k0)
//{
// //ellipsoid parameters(WGS84):
// double ref_A = 6378137.0; ///< equatorial radius
// double ref_f = 1.0/298.257223563;
// double ref_B = ref_A*(1-ref_f);
// double ref_R = 6371010.0;
//// f = (A-B)/A; //flattening
// double ref_e = sqrt(2*ref_f-ref_f*ref_f); //the first eccentricity
// double ref_A_B = ref_A/ref_B;
// double ref_B_A = ref_B/ref_A;
// double ref_B2_A2 = (ref_B*ref_B)/(ref_A*ref_A);
// double ref_A2_B2 = 1.0 / ref_B2_A2;
// double ref_e2 = ref_e*ref_e;
// double ref_e1 = (ref_A-ref_B)/(ref_A+ref_B);
// double ref_ep = sqrt(ref_A*ref_A-ref_B*ref_B)/ref_B; //the second eccentricity
// double ref_ep2 = ref_ep*ref_ep;
// // utm2ll constants
// f8 ref_m_0,ref_m_1,ref_m_2,ref_m_3;
// f8 m0,m2,m4,m6,m8;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -