geopos.cpp

GPS坐标转换软件与源程序 支持世界上大多数坐标框架下

//	m0		= ref_A*(1-ref_e2);		
//	m2		= 3.0/2*ref_e2*m0;
//	m4		= 5.0/4*ref_e2*m2;
//	m6		= 7.0/6*ref_e2*m4;
//	m8		= 9.0/8*ref_e2*m6;	
//	ref_m_0		= m0+m2/2.0+(3.0/8)*m4+(5.0/16)*m6+(35.0/128)*m8;
//	ref_m_1		= m2/2+m4/2+(15.0/32)*m6+(7.0/16)*m8;
//	ref_m_2		= m4/8+(3.0/16)*m6+(7.0/32)*m8;
//	ref_m_3		= m6/32.0+m8/16.0;
//
//
//	// central-meridian scale factor
//	double lon0 = 0.0;
//	double nu, T, T2, C, CP2, CP4,A,A2,A3,A4,M,S;
//	utm[2]= alt;
//	
//	//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;
//	}
//
//  	// 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;
//	} else
//  	// special zones for Svalbard, lat 72 - 84
//	if( lat >= 1.2566370614359 && lat < 1.4660765716752) {
//		if (lon < 0.0) {
//		} else
//		if (lon < 0.157079632679490) { // 0 - 9 deg
//			_zone = 31;
//			lon0 = 0.078539816339744;
//		} else
//		if (lon < 0.366519142918809) { // 9 - 21 deg
//			_zone = 33;
//			lon0 = 0.261799387799149;
//		} else
//		if (lon < 0.575958653158129) { // 21 - 33 deg
//			_zone = 35;
//			lon0 = 0.471238898038469;
//		} else
//		if (lon < 0.733038285837618) { // 33 - 42 deg
//			_zone = 37;
//			lon0 = 0.654498469497874;
//		}
//	}
//	//evaluate the value of zone
//	_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);
//	
//
//	utm[0] = 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;
//	utm[1] =  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) {
//		utm[1] += 10000000.0; //10000000 meter offset for southern hemisphere
//	}
//}

void SurfaceDistance(LLA const &p, LLA const &q, double &distance, double &bearing, CReferenceEllipsoid const &_ref)
{
	double U1 = atan2((1-_ref.f) * sin(p.latitude()), cos(p.latitude()));
	double U2 = atan2((1-_ref.f) * sin(q.latitude()), cos(q.latitude()));
	double s_U1 = sin(U1);
	double c_U1 = cos(U1);
	double s_U2 = sin(U2);
	double c_U2 = cos(U2);
	double psU = s_U1 * s_U2;
	double pcU = c_U1 * c_U2;
	double L = q.longitude() - p.longitude();
	double lam = L;
	double last = 100.0;
	double s_lam;
	double c_lam;
	double t1;
	double t2;
	double s2_sig=0.0;
	double c_sig=0.0;
	double sig=0.0;
	double s_sig;
	double s_alp;
	double c2_alp;
	double c_2sigm=0.0;
	double u;
	double u2=0.0;
	double c;
	while (fabs(lam - last) > 0.00001) {
		last = lam;
		s_lam = sin(lam);
		c_lam = cos(lam);
		t1 = c_U2 * s_lam;
		t2 = c_U1 * s_U2 - s_U1 * c_U2 * c_lam;
		s2_sig = t1*t1 + t2*t2;
		c_sig = s_U1 * s_U2 + c_U1 * c_U2 * c_lam;
		sig = acos(c_sig);
		s_sig = sqrt(s2_sig);
		if (s2_sig == 0.0) {
			if (pcU * s_lam > 0.0) {
				s_alp = 1.0;
			} else
			if (pcU * s_lam < 0.0) {
				s_alp = -1.0;
			} else {
				s_alp = 0.0;
			}
		} else {
			s_alp = pcU * s_lam / s_sig;
			s_alp = min(1.0, max(-1.0, s_alp));
		}
		c2_alp = 1.0 - s_alp*s_alp;
		if (c2_alp == 0.0) {
			if (psU > 0.0) {
				c_2sigm = 1.0;
			} else
			if (psU < 0.0) {
				c_2sigm = -1.0;
			} else {
				c_2sigm = 0.0;
			}
		} else {
			c_2sigm = c_sig - 2.0 * psU / c2_alp;
			c_2sigm = min(1.0, max(-1.0, c_2sigm));
		}
		u = cos(asin(s_alp));
		u2 = u * u * (_ref.A2_B2 - 1);
		c = _ref.f * c2_alp * (4.0 + _ref.f * (4.0 - 3.0 * c2_alp)) / 16.0;
		lam = L + (1.0 - c) * _ref.f * s_alp * (sig + c * s_sig * (c_2sigm + c * c_sig * (-1.0 + 2.0 * c_2sigm * c_2sigm)));
	}
	double a = 1.0 + u2 * (64.0 + u2 * (-12.0 + 5.0 * u2)) / 256.0;
	double b = u2 * (128.0 + u2 * (-64.0 + 37.0 * u2)) / 512.0;
	double dsig = b * sqrt(s2_sig) * (c_2sigm + 0.25 * b * c_sig * (-1.0 + 2.0 * c_2sigm * c_2sigm));
	distance = _ref.B * a * (sig - dsig);
	bearing = atan2(c_U2 * sin(lam), c_U1 * s_U2 - s_U1 * c_U2 * cos(lam));
}


void ShellDistance(LLA const &p, LLA const &q, double &distance, double &bearing, CReferenceEllipsoid const &_ref)
{
	double da = p.altitude() - q.altitude();
        SurfaceDistance(p, q, distance, bearing, _ref);
	distance = sqrt(distance*distance + da*da);
}


////////////////////////////////////////////////////////////////////////////////////
// GRID
/**
 * Get the GRID designator for a given latitude, or 'Z' if outside
 * the GRID limits (80S to 84N)
 *
 * @param latitude latitude in radians
 */
char GRID::GetDesignator(double latitude)
{
	static const char designator[] = "CDEFGHJKLMNPQRSTUVWXX";
	latitude = toDegrees(latitude);
	if (latitude < -80.0 || latitude > 84.0) return 'Z';
	return designator[(int)(latitude + 80.0)>>3];
}
//	//////////////////////////////////////////////////////////////////////////
//	/* 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. 
//	*/
void GRID::GetZoneAndCM(double lon, double lat,char& zone,double& lon0)
{	
	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
		}
	}
	else{
		zone = char((lon / PI + 1.0) * 30.0) + 1;
		lon0 = toRadians((int(zone) - 1)*6.0 - 180.0 + 3.0);
	}

}

void GRID::Set(double easting_, double northing_, const double lon0,
			   double alt /* = 0.0 */, const char NorS/* ='N' */)
{
	_zone=0;
	_designator='Z';
	_E = easting_;
	_N = northing_;
	_alt = alt;
	_NorS = NorS;
	_lon0 = lon0;
	
}
void GRID::Get(double& easting, double& northing,char& zone, double& alt)
{
	easting = this->easting();
	northing = this->northing();
	alt = this->altitude();
	zone = this->zone();
}

bool GRID::valid() const {
	if (!strchr("CDEFGHJKLMNPQRSTUVWXX", _designator)) {
		return false;
	}
	// check ZONE
	return true;
}

std::string GRID::asString() const
{
	std::ostringstream ss;
	ss << std::fixed;
	ss << "[";
	ss << ss.precision(0) << _E << " " << _N;
	ss << " " << int(_zone) << _designator << ", ";
	ss << std::setprecision(3) << _alt;
	ss << "]";
	return ss.str();
}

void GRID::parseXML(const char *cdata)
{
	if (cdata) {
		const char *c = cdata;
		while (*c != 0 && (*c == ' ' || *c == '\t' || *c == '\r' || *c == '\n')) c++;
		int zone_;
		char designator_;
		int n = sscanf(c, "%lf %lf %d%c %lf", &_E, &_N, &zone_, &designator_, &_alt);
		if (n != 5) throw std::string("SYNTAX ERROR: expecting 'easting northing zone altitude'");
		_zone = static_cast<char>(zone_);
		_designator = static_cast<char>(toupper(designator_));
		if (!valid()) {
			throw std::string("SYNTAX ERROR: invalid GRID code");
		}
	} else throw std::string("SYNTAX ERROR: empty vector");
}

#ifdef WIN32
void GRID::serialize(CArchive* ar) {
	if(ar->IsLoading())
		(*ar) >> _E >> _N >> _zone >> _designator >> _alt;
	else
		(*ar) << _E << _N << _zone << _designator << _alt;
}
#endif


////////////////////////////////////////////////////////////////////////////////////
// LLA

std::string LLA::asString() const
{
	std::ostringstream ss;
	ss << std::fixed << std::setprecision(3);
	ss << "[" << toDegrees(_lat)
	   << " " << toDegrees(_lon)
	   << ", " << _alt
	   << "]";
	return ss.str();
}
void LLA::parseXML(const char* cdata) {
	// 37.91283 -120.12398 234.033
	// 37'59"20 -120'4"43 234.033
	if (cdata) {
		const char *c = cdata;
		while (*c != 0 && (*c == ' ' || *c == '\t' || *c == '\r' || *c == '\n')) c++;
		double X, Y, Z;
		if (strchr(c, '\'')) {
			int lat, latm, lon, lonm;
			int n = sscanf(c, "%d'%d\"%lf %d'%d\"%lf %lf", &lat, &latm, &X, &lon, &lonm, &Y, &Z);
			if (n != 7) throw std::string("SYNTAX ERROR: expecting 'lat'min\"sec lon'min\"sec altitude'");
			_lat = toRadians(lat + latm / 60.0 + X / 3600.0);
			_lon = toRadians(lon + lonm / 60.0 + Y / 3600.0);
			_alt = Z;
		} else {
			int n = sscanf(c, "%lf %lf %lf", &X, &Y, &Z);
			if (n != 3) throw std::string("SYNTAX ERROR: expecting 'latitude longitude altitude'");
			_lat = toRadians(X);
			_lon = toRadians(Y);
			_alt = Z;
		}
		return;
	}
	throw std::string("SYNTAX ERROR: expecting 'latitude longitude altitude'");
}

#ifdef WIN32
void LLA::serialize(CArchive* ar) {
	if(ar->IsLoading())
		(*ar) >> _lat >> _lon >> _alt;
	else
		(*ar) << _lat << _lon << _alt;
}
#endif

void LLA::SetInDegrees(double lat, double lon, double alt) {
	_lat = toRadians(lat);
	_lon = toRadians(lon);
	_alt = alt;
}

void LLA::GetInDegrees(double& lat, double& lon, double& alt){
	lat = latitudeInDegrees();
	lon = longitudeInDegrees();
	alt = _alt;
}

void LLA::Get(double& lat, double& lon,double& alt,char unittype){
	switch(unittype) {
	case 0:
		break;
	case 1:
		this->GetInDegrees(lat,lon,alt);
		break;
	case 2:
		lat=_lat;lon=_lon;alt=_alt;
		break;
	case 3:
		this->GetInDegrees(lat,lon,alt);
		lat *= 3600;
		lon *= 3600;
		break;
	}
}

void LLA::Set(double lat, double lon, double alt,char unittype) {
	switch(unittype) {
	case 0:
		break;
	case 1:
		SetInDegrees(lat,lon,alt);
		break;
	case 2:
		Set(lat,lon,alt);
		break;
	case 3:
		SetInDegrees(lat/3600,lon/3600,alt/3600);
		break;
	}		
}

void LLA::Set(double lat, double lon, double alt/* =0.0  */)
{
	_lat = lat;
	_lon = lon;
	_alt = alt;
}
////////////////////////////////////////////////////////////////////////////////////
// ECEF


std::ostream &operator <<(std::ostream &o, LLA const &q) { return o << q.asString(); }
std::ostream &operator <<(std::ostream &o, GRID const &q) { return o << q.asString(); }
//std::ostream &operator <<(std::ostream &o, ECEF const &q) { return o << q.asString(); }