#pragma once
#include <vector>
#include <cmath>
using namespace std;
#define PI 3.14159265358979323846
class GeoCompute
{
public:
	GeoCompute(void);
	~GeoCompute(void);

	/*@brief  根据起点坐标、方位角、距离，计算另一点坐标。
	* 使用Vincenty's公式求解,使用WGS-84椭球
	* startPoint:起始点地理坐标点（lat(-90到90)，lon(-180,180)）
	* bearing:方位角（度）
	* dist:两点之间距离(km)
	*/
	void GeoCompute::computeOffsetGeoPosition(double lon1, double lat1, double bearing, double dist,double& targetLon,double& targetLat)
	{
		//角度转化为弧度
		//    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椭球参数
		double flat = 298.257223563;
		double a = 6378137.0;
		double b = 6356752.314245;
		//开始求解坐标
		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;
	}
};