通过经纬度计算两车的距离和相对角度

将两点的经纬度作为四个参数，利用 Haversine公式：

其中：

• R为地球半径，可取平均值 6371km；φ1, φ2 表示两点的纬度；Δλ 表示两点经度的差值。

 代码实现：

double Distance(double lat1, double lat2, double lng1, double lng2)
{
	double radlat1 = lat1 * Rad;

	double radlat2 = lat2 * Rad;

	double a = radlat1 - radlat2;

	double b = (lng1 - lng2) * Rad;

	double distance = (2.0 * R * asin(sqrt(pow(sin(a / 2), 2) + cos(radlat1) * cos(radlat2) * pow(sin(b / 2), 2)))); // point to point  Distance   

	//double distance1 = distance - (hv_length +  rv.length)/2//  distance1 is  longitudinal distance . assuming.


	return distance;
}

得到两点的直线距