根据GPS计算两点间的距离和角度

借鉴别人的博客完成两个任务：
1、根据两个gps的信息计算距离和角度
2、根据一个gps点和距离、角度进而求终点的位置
博客地址：
添加链接描述
添加链接描述
添加链接描述
C++代码：
头文件：

#ifndef MYBIGCIRCLECLASS_H
#define MYBIGCIRCLECLASS_H
#pragma once
#include<math.h>
#define _PI 3.1415926
#define PI_180 0.01745329   //pi/180  角度转弧度
#define Earth_R 6372.795477598
class MyBigCircleClass
{
public:
    MyBigCircleClass();
    //根据两点间GPS计算两点的距离和方位
    void getDistanceBearing(double StartLat,double StartLng, double EndLat,double EndLng, double& dis,double& bearing);
	//根据两个点计算中间点的位置
    void getMidPoint(double StartLat,double StartLng, double EndLat,double EndLng,double& MidLat,double& MidLng);
    //根据一个点的GPS和距离、方位计算另外一个点的GPS位置
    void getEndPoint(double StartLat,double StartLng,double dis, double bearing,double& EndLat,double& EndLng);
};

#endif // MYBIGCIRCLECLASS_H

实现cpp：

#include "MyBigCircleClass.h"

MyBigCircleClass::MyBigCircleClass()
{

}
/*函数作用：根据两点间GPS计算两点的距离和方位
 *输入参数：起点的经纬度；终点的经纬度：
 *函数输出：两点的距离dis和方位bearing
 *备注：       */
void MyBigCircleClass::getDistanceBearing(double StartLat,double StartLng, double EndLat,double EndLng, double& dis,double& bearing)
{
    //Fai对应纬度 Lat，temp对应经度 lon
    double Fai1 = StartLat * PI_180;
    double Fai2 = EndLat * PI_180;
    double temp = (EndLng - StartLng) * PI_180;

    dis = Earth_R * acos(sin(Fai1) * sin(Fai2) + cos(Fai1)*cos(Fai2)*cos(temp)) * 1000;//已经换算成m了
    bearing = atan2(sin(temp)*cos(Fai2),cos(Fai1)*sin(Fai2)-sin(Fai1)*cos(Fai2)*cos(temp))/PI_180;//弧度
	if(bearing < 0)
		bearing += 360;

}
/*函数作用：根据两点间GPS计算中间点的位置
 *输入参数：
 *函数输出：
 *备注：起点终点数组中0号元素为纬度Lat，1号元素为经度Lng       */
void MyBigCircleClass::getMidPoint(double StartLat, double StartLng, double EndLat, double EndLng, double& MidLat, double& MidLng)
{
    //Fai对应纬度 Lat，temp对应经度 lon
    double Fai1 = StartLat * PI_180;
    double Fai2 = EndLat * PI_180;
    double temp = (EndLng - StartLng) * PI_180;

    double Bx = cos(Fai2)*cos(temp);
    double By = cos(Fai2)*sin(temp);
    double Tmp = sqrt((cos(Fai1)+Bx)*(cos(Fai1)+Bx) + By*By);

    MidLat = atan2(sin(Fai1)+sin(Fai2), Tmp)/PI_180;

    MidLng = StartLat + atan2(By, cos(Fai1)+Bx)/PI_180;
	MidLng = fmod(MidLng+540, 360)-180;
}
/*函数作用：根据一个点的GPS和距离、角度计算终点的GPS
 *输入参数：
 *函数输出：
 *备注：起点终点数组中0号元素为纬度Lat，1号元素为经度Lng       */
void MyBigCircleClass::getEndPoint(double StartLat,double StartLng, double dis, double bearing, double& EndLat,double& EndLng)
{
    double Fai1 = StartLat * PI_180;
    double scale = dis / Earth_R/1000;
    double bear = bearing * PI_180;
    EndLat = asin(sin(Fai1)*cos(scale)+cos(Fai1)*sin(scale)*cos(bear));

    EndLng = StartLng + atan2(sin(bear)*sin(scale)*cos(Fai1), cos(scale)-sin(Fai1)*sin(EndLat))/PI_180;

	EndLat = EndLat/PI_180;
	EndLng = fmod(EndLng+540, 360)-180;
}