GCS-GISControlDlg-for-981A-and-981C/geocompute.cpp

#include "stdafx.h"
#include "geocompute.h"


GeoCompute::GeoCompute(void)
{
}


GeoCompute::~GeoCompute(void)
{
}

/*@brief  <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ꡢ<EFBFBD><EAA1A2>λ<EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD>룬<EFBFBD><EBA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ꡣ
* ʹ<EFBFBD><EFBFBD>Vincenty's<EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>,ʹ<EFBFBD><EFBFBD>WGS-84<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* startPoint:<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣨lat(-90<EFBFBD><EFBFBD>90)<EFBFBD><EFBFBD>lon(-180,180)<EFBFBD><EFBFBD>
* bearing:<EFBFBD><EFBFBD>λ<EFBFBD>ǣ<EFBFBD><EFBFBD>ȣ<EFBFBD>
* dist:<EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(km)
*/
void GeoCompute::computeOffsetGeoPosition(double lon1, double lat1, double bearing, double dist,double& targetLon,double& targetLat)
{
	//<2F>Ƕ<EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
	//    qreal lon1 = (fmod(startPoint.x()+540,360)-180.0)*PI/180;
	lon1 = lon1*PI/180.0;
	lat1 = lat1*PI/180.0;
	bearing = bearing*PI/180.0;
	dist = dist*1000;

	//WGS-84<38><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double flat = 298.257223563;
	double a = 6378137.0;
	double b = 6356752.314245;
	//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double f = 1/flat;
	double sb = sin(bearing);
	double cb = cos(bearing);
	double tu1 = (1-f)*tan(lat1);
	double cu1 = 1/sqrt((1+tu1*tu1));
	double su1 = tu1*cu1;
	double s2 = atan2(tu1, cb);
	double sa = cu1*sb;
	double csa = 1-sa*sa;
	double us = csa*(a*a - b*b)/(b*b);
	double A = 1+us/16384*(4096+us*(-768+us*(320-175*us)));
	double B = us/1024*(256+us*(-128+us*(74-47*us)));
	double s1 = dist/(b*A);
	double s1p = 2*PI;


	double cs1m = 0.0;
	double ss1 = 0.0;
	double cs1 = 0.0;
	double ds1 = 0.0;

	while (abs(s1-s1p) > 1e-12)
	{
		cs1m = cos(2*s2+s1);
		ss1 = sin(s1);
		cs1 = cos(s1);
		ds1 = B*ss1*(cs1m+B/4*(cs1*(-1+2*cs1m*cs1m)- B/6*cs1m*(-3+4*ss1*ss1)*(-3+4*cs1m*cs1m)));
		s1p = s1;
		s1 = dist/(b*A)+ds1;
	}

	double t = su1*ss1-cu1*cs1*cb;
	double lat2 = atan2(su1*cs1+cu1*ss1*cb, (1-f)*sqrt(sa*sa + t*t));
	double l2 = atan2(ss1*sb, cu1*cs1-su1*ss1*cb);
	double c = f/16*csa*(4+f*(4-3*csa));
	double l = l2-(1-c)*f*sa* (s1+c*ss1*(cs1m+c*cs1*(-1+2*cs1m*cs1m)));
	double lon2 = lon1+l;

	targetLon = lon2*180/PI;
	targetLat = lat2*180/PI;
}

	// ʹ<><CAB9>Vincenty's<><73>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double GeoCompute::VincentyDistance(double lon1, double lat1, double lon2, double lat2) {
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	const double a = 6378137.0;       // <20><><EFBFBD>򳤰<EFBFBD><F2B3A4B0><EFBFBD> (<28><>)
	const double f = 1 / 298.257223563; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	const double b = a * (1 - f);     // <20><><EFBFBD><EFBFBD><EFBFBD>̰<EFBFBD><CCB0><EFBFBD> (<28><>)


	lat1 = lat1* M_PI / 180.0;
	lon1 = lon1* M_PI / 180.0;
	lat2 = lat2* M_PI / 180.0;
	lon2 = lon2* M_PI / 180.0;

	double L = lon2 - lon1;
	double U1 = atan((1 - f) * tan(lat1));
	double U2 = atan((1 - f) * tan(lat2));
	double sinU1 = sin(U1), cosU1 = cos(U1);
	double sinU2 = sin(U2), cosU2 = cos(U2);

	double lambda = L, lambdaP;
	int iterLimit = 100;
	double sinLambda, cosLambda;
	double sinSigma, cosSigma, sigma;
	double sinAlpha, cos2Alpha, cos2SigmaM;
	double C;

	do {
		sinLambda = sin(lambda);
		cosLambda = cos(lambda);
		sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
			(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
		if (sinSigma == 0) return 0; // co-incident points
		cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
		sigma = atan2(sinSigma, cosSigma);
		sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
		cos2Alpha = 1 - sinAlpha * sinAlpha;
		cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cos2Alpha;
		if (cos2Alpha == 0) cos2SigmaM = 0; // equatorial line: cos2Alpha=0
		C = f / 16 * cos2Alpha * (4 + f * (4 - 3 * cos2Alpha));
		lambdaP = lambda;
		lambda = L + (1 - C) * f * sinAlpha *
			(sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
	} while (fabs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

	if (iterLimit == 0) return -1; // formula failed to converge, return -1 or an appropriate error code

	double uSquared = cos2Alpha * (a * a - b * b) / (b * b);
	double A = 1 + uSquared / 16384 * (4096 + uSquared * (-768 + uSquared * (320 - 175 * uSquared)));
	double B = uSquared / 1024 * (256 + uSquared * (-128 + uSquared * (74 - 47 * uSquared)));
	double deltaSigma = B * sinSigma *
		(cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
		B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));

	double s = b * A * (sigma - deltaSigma);

	return s;
}