根据两点坐标获取航向角和距离的c程序
#include <stdio.h>
#include <math.h>
#define PI 3.1415926
#define EARTH_RADIUS 6378.137 //地球近似半径
double radian(double d);
double get_distance(double lat1, double lng1, double lat2, double lng2);
// 求弧度
double radian(double d)
{
return d * PI / 180.0; //角度1˚ = π / 180
}
//计算距离
double get_distance(double lat1, double lng1, double lat2, double lng2)
{
double radLat1 = radian(lat1);
double radLat2 = radian(lat2);
double a = radLat1 - radLat2;
double b = radian(lng1) - radian(lng2);
double dst = 2 * asin((sqrt(pow(sin(a / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(b / 2), 2) )));
dst = dst * EARTH_RADIUS;
dst= round(dst * 10000) / 10000;
return dst;
}
int get_bearing(double lat1, double lng1, double lat2, double lng2)
{
int distance_North = EARTH_RADIUS*(lat2-lat1);
int distance_East = EARTH_RADIUS*cos(lat1)*(lng2-lng1);
// int bearing = modf( atan2(distance_East, distance_North), 2*PI );
// return bearing;
}
double toRadian(double point)
{
return (point*M_PI)/180.0;
}
double toDegree(double point)
{
return point*180.0/ M_PI;
}
int toBearing(double angle)
{
double bearing = toDegree(angle)+ 360.0;
return fmod(bearing, 360);
}
/*
This function will determine the bearing of the path to be travelled
*/
double getBearing(double longtitude_start, double latitude_start, double longtitude_end, double latitude_end)
{
double rad_latitude_start = toRadian(latitude_start);
double rad_latitude_end = toRadian(latitude_end);
double deltaLongtitude = toRadian(longtitude_end)- toRadian(longtitude_start);
double x = sin(deltaLongtitude)*cos(rad_latitude_end);
double y = cos(rad_latitude_start)*sin(rad_latitude_end)-sin(rad_latitude_start)*cos(rad_latitude_end)*cos(deltaLongtitude);
return toBearing(atan2(x, y));
}
double getAngle1(double lat_a, double lng_a, double lat_b, double lng_b)
{
double y = sin(lng_b-lng_a) * cos(lat_b);
double x = cos(lat_a)*sin(lat_b) - sin(lat_a)* cos(lat_b)* cos(lng_b-lng_a);
double bearing = atan2(y, x);
// bearing = Math.toDegrees(bearing);
if(bearing < 0){
bearing = bearing +360;
}
return bearing;
}
//heading ：车的航向角相对于真北方向
// bearing： 目的地相对于真北方向的方位角
//返回：最优的转弯方案：转弯方向，转弯角度，行驶距离
int HeadingAnalysis(int Heading,int Bearing)
{
int headingtmp;
if((Heading - Bearing)< 0)
{
headingtmp = Heading +360;
if(headingtmp - Bearing <= 180)
{
//左转
printf("turn left %d\n",headingtmp - Bearing);
}else if(headingtmp - Bearing > 180){
printf("turn right %d\n",360- (headingtmp-Bearing));
}
}else if((Heading - Bearing) > 0)
{
if(Heading - Bearing < 180)
{
printf("turn left %d\n",Heading - Bearing);
}
else if(Heading - Bearing > 180)
{
printf("turn right %d \n",360-(Heading-Bearing));
}
}
}
int main (int argc, const char * argv[])
{
double lat1 = 39.90759;
double lng1 = 116.41552;//经度,纬度1
double lat2 = 39.93804;
double lng2 = 116.41371;//经度,纬度2
// insert code here...
int angle;
double dst = get_distance(lat1, lng1, lat2, lng2);
angle= getBearing( lng1,lat1, lng2 ,lat2);
printf("dst = %0.3fkm\n", dst); //dst = 9.281km
// printf("angle = %d 度\n", angle);
HeadingAnalysis(70,150);
return 0;
}
该程序已经在Linux下验证通过可行，该程序和算法将最终用到GPS定位小车上。
