基于java的生成大疆弓形航线算法实现

问题背景：本人在开发过程中，遇到给定两个点的坐标，需要生成大量的有规律的坐标（实际上就是大疆无人机的航线）。查询相关资料发现国内的博客基本都是计算两点距离，计算点到线距离的距离等，有几篇也都是C语言形式。在这些资料的基础上本人继续研究深入，最终调试出了符合本人的业务场景的算法，在这里分享给予大家参考，算法精度到米级别

下面是算法实现的生成两张效果图：

A点为起始坐标 B点为终点坐标 S为开始点 G为终点 下图为横向的方式生成 本文称为 短边方向

按照 长边 的方向生成的效果图为

下面为算法的主要实现代码：

import com.jzi.cloud.api.entity.vo.CoordinateVo;
import org.springframework.stereotype.Component;

import java.util.ArrayList;
import java.util.List;

/**
 * 地理坐标处理工具类
 * */
@Component
public class GeographicalCoordinatesUtils {

	/**
	 * 地球半径（这里取的是平均半径）
	 */
	private static final double EARTH_RADIUS = 6.371229 * 1e6;

	/**
	 * 角度弧度计算公式 rad:()
	 * 360度=2π π=Math.PI
	 * x度 = x*π/360 弧度
	 */
	private static double getRadian(double degree) {
		return degree * Math.PI / 180.0;
	}

	/**
	 * 弧度换成度
	 * @param radian 弧度
	 * @return degree 度
	 */
	public static double getDegree(double radian) { return radian * 180 / Math.PI; }

	/**
	 * 依据经纬度计算两点之间的距离 GetDistance:()
	 * @param lng1 地点A的经度
	 * @param lat1 地点A的纬度
	 * @param lng2 地点B的经度
	 * @param lat2 地点B的纬度
	 * @return 返参 double 单米（m）
	 */
	public static double getDistance(double lng1, double lat1, double lng2, double lat2) {
		double radLat1 = getRadian(lat1);
		double radLat2 = getRadian(lat2);
		double a = radLat1 - radLat2;// 两点纬度差
		double b = getRadian(lng1) - getRadian(lng2);// 两点的经度差
		double s = 2 * Math.asin(Math.sqrt(Math.pow(Math.sin(a / 2), 2) + Math.cos(radLat1)
				* Math.cos(radLat2) * Math.pow(Math.sin(b / 2), 2)));
		s = s * EARTH_RADIUS;
		return s;
	}

	/*
	 * 大地坐标系资料WGS-84 长半径a=6378137 短半径b=6356752.3142 扁率f=1/298.2572236
	 */
	/** 长半径a=6378137 */
	private static final double a = 6378137;
	/** 短半径b=6356752.3142 */
	private static final double b = 6356752.3142;
	/** 扁率f=1/298.2572236 */
	private static final double f = 1 / 298.2572236;

	/**
	 * 通过一个点坐标计算另一点经纬度
	 * @param lon  经度
	 * @param lat  维度
	 * @param brng 方位角(传入角度)
	 * @param dist 距离（米）
	 * @return  double[] 经纬度
	 */
	public static double[] computerThatLonLat(double lon, double lat, double brng, double dist) {

		double alpha1 = getRadian(brng);
		double sinAlpha1 = Math.sin(alpha1);
		double cosAlpha1 = Math.cos(alpha1);

		double tanU1 = (1 - f) * Math.tan(getRadian(lat));
		double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1));
		double sinU1 = tanU1 * cosU1;
		double sigma1 = Math.atan2(tanU1, cosAlpha1);
		double sinAlpha = cosU1 * sinAlpha1;
		double cosSqAlpha = 1 - sinAlpha * sinAlpha;
		double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
		double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
		double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));

		double cos2SigmaM=0;
		double sinSigma=0;
		double cosSigma=0;
		double sigma = dist / (b * A), sigmaP = 2 * Math.PI;
		while (Math.abs(sigma - sigmaP) > 1e-12) {
			cos2SigmaM = Math.cos(2 * sigma1 + sigma);
			sinSigma = Math.sin(sigma);
			cosSigma = Math.cos(sigma);
			double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
					- B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
			sigmaP = sigma;
			sigma = dist / (b * A) + deltaSigma;
		}

		double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
		double lat2 = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
				(1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp));
		double lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
		double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
		double L = lambda - (1 - C) * f * sinAlpha
				* (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
		// final bearing
		double revAz = Math.atan2(sinAlpha, -tmp);

		double[] coordinates = new double[2];
		coordinates[0] = lon+getDegree(L);
		coordinates[1] = getDegree(lat2);
		return coordinates;
	}

	//传入两点，使用默认的宽度(10m)、间隔（10m）、高度200、长边生成航点
	public static List<CoordinateVo> defaultGenerateCoordinates(double lng1, double lat1, double lng2, double lat2){
		return coordinateProcessing(lng1, lat1, lng2, lat2, 10, 10,200,true);
	}

	//传入两点，自定义的宽度、间隔、高度、长（短）边生成航点
	public static List<CoordinateVo> customGenerateCoordinates(double lng1, double lat1, double lng2, double lat2,
																float width,float interval,float high,boolean direction){
		return coordinateProcessing(lng1, lat1, lng2, lat2, width, interval,high,direction);
	}


	/**
	 * 返回处理后的航点坐标的集合
	 * width  宽度
	 * interval 间隔距离
	 * high 航点高度
	 * direction true为长边，false为短边
	 */
	private static List<CoordinateVo> coordinateProcessing(double lng1, double lat1, double lng2, double lat2,
														   float width,float interval,float high,boolean direction ){
		//两点间距离
		double distance = getDistance(lng1, lat1, lng2, lat2);
		//计算偏移角
		double offsetAngle= Math.toDegrees(Math.atan((lng1-lng2)/(lat1-lat2)));
		//计算单边的航点数量
		int divisionNum;
		List<CoordinateVo> oneCoordinateVoList;
		List<CoordinateVo> twoCoordinateVoList;
		List<CoordinateVo> coordinateVos;
		CoordinateVo coordinateVo1 ;
		CoordinateVo coordinateVo2 ;
		
		if (direction){
			//长边
			divisionNum = (int) Math.ceil( (width*2) / interval) + 1;
			oneCoordinateVoList = new ArrayList<>(divisionNum); //上
			twoCoordinateVoList = new ArrayList<>(divisionNum); //下
			coordinateVos = new ArrayList<>(divisionNum * 2);

			int temp = divisionNum / 2 ;
			//处理点左边
			for (int i = temp; i >= 0; i--) {
				coordinateVo1 = new CoordinateVo();
				coordinateVo2 = new CoordinateVo();
				//上面左边
				double[] leftCoordinateVo = computerThatLonLat(lng2, lat2, -90+offsetAngle, i*interval);
				coordinateVo1.setLongitude(leftCoordinateVo[0]);
				coordinateVo1.setLatitude(leftCoordinateVo[1]);
				coordinateVo1.setAltitude((double) high);
				oneCoordinateVoList.add(coordinateVo1);
				//下面左边
				double[] rightCoordinateVo = computerThatLonLat(lng1, lat1, -90+offsetAngle, i*interval);
				coordinateVo2.setLongitude(rightCoordinateVo[0]);
				coordinateVo2.setLatitude(rightCoordinateVo[1]);
				coordinateVo2.setAltitude((double) high);
				twoCoordinateVoList.add(coordinateVo2);
			}
			//处理点右边
			for (int i = 1; i <= temp ; i++) {
				coordinateVo1 = new CoordinateVo();
				coordinateVo2 = new CoordinateVo();
				//上
				double[] leftCoordinateVo = computerThatLonLat(lng2, lat2, 90+offsetAngle, i*interval);
				coordinateVo1.setLongitude(leftCoordinateVo[0]);
				coordinateVo1.setLatitude(leftCoordinateVo[1]);
				coordinateVo1.setAltitude((double) high);
				oneCoordinateVoList.add(coordinateVo1);
				//下
				double[] rightCoordinateVo = computerThatLonLat(lng1, lat1, 90+offsetAngle, i*interval);
				coordinateVo2.setLongitude(rightCoordinateVo[0]);
				coordinateVo2.setLatitude(rightCoordinateVo[1]);
				coordinateVo2.setAltitude((double) high);
				twoCoordinateVoList.add(coordinateVo2);
			}
			//如果A点在上面则交换两个集合的数据
			if (lat1>lat2){
				List<CoordinateVo> tempList=new ArrayList<>(oneCoordinateVoList);
				oneCoordinateVoList = new ArrayList<>(twoCoordinateVoList);
				twoCoordinateVoList = new ArrayList<>(tempList);
			}
			//点排序
			boolean flag=false;
			for (int i = 0; i < oneCoordinateVoList.size(); i++) {
				if (flag){
					//从下往上
					coordinateVos.add(twoCoordinateVoList.get(i));
					coordinateVos.add(oneCoordinateVoList.get(i));
					flag =false;
				}else {
					//从上往下
					coordinateVos.add(oneCoordinateVoList.get(i));
					coordinateVos.add(twoCoordinateVoList.get(i));
					flag = true;
				}
			}
		}else {
			//短边
			divisionNum = (int) Math.ceil(distance / interval) + 1;
			oneCoordinateVoList = new ArrayList<>(divisionNum); //保存左侧的所有点
			twoCoordinateVoList = new ArrayList<>(divisionNum); //保存右侧的所有点
			coordinateVos = new ArrayList<>(divisionNum * 2);

			//计算第一个点左边的坐标
			double[] leftCoordinate = computerThatLonLat(lng1, lat1, -90+offsetAngle, width);
			//计算第一个点右边的坐标
			double[] rightCoordinate = computerThatLonLat(lng1, lat1, 90+offsetAngle, width);

			//判断哪个点在上方
			if (lat1<lat2){ //A下B上
				//计算出所有点并且进行左右分类
				for (int i = 0; i < divisionNum; i++) {
					coordinateVo1 = new CoordinateVo();
					coordinateVo2 = new CoordinateVo();
					//左边
					double[] leftCoordinateVo = computerThatLonLat(leftCoordinate[0], leftCoordinate[1], 0+offsetAngle, i*interval);
					coordinateVo1.setLongitude(leftCoordinateVo[0]);
					coordinateVo1.setLatitude(leftCoordinateVo[1]);
					coordinateVo1.setAltitude((double) high);
					oneCoordinateVoList.add(coordinateVo1);
					//右边
					double[] rightCoordinateVo = computerThatLonLat(rightCoordinate[0], rightCoordinate[1], 0+offsetAngle, i*interval);
					coordinateVo2.setLongitude(rightCoordinateVo[0]);
					coordinateVo2.setLatitude(rightCoordinateVo[1]);
					coordinateVo2.setAltitude((double) high);
					twoCoordinateVoList.add(coordinateVo2);
				}
			}else {//A上B下
				for (int i = 0; i < divisionNum; i++) {
					coordinateVo1 = new CoordinateVo();
					coordinateVo2 = new CoordinateVo();
					//左边
					double[] leftCoordinateVo = computerThatLonLat(leftCoordinate[0], leftCoordinate[1], 180+offsetAngle, i*interval);
					coordinateVo1.setLongitude(leftCoordinateVo[0]);
					coordinateVo1.setLatitude(leftCoordinateVo[1]);
					coordinateVo1.setAltitude((double) high);
					oneCoordinateVoList.add(coordinateVo1);
					//右边
					double[] rightCoordinateVo = computerThatLonLat(rightCoordinate[0], rightCoordinate[1], 180+offsetAngle, i*interval);
					coordinateVo2.setLongitude(rightCoordinateVo[0]);
					coordinateVo2.setLatitude(rightCoordinateVo[1]);
					coordinateVo2.setAltitude((double) high);
					twoCoordinateVoList.add(coordinateVo2);
				}
			}
			//点排序
			boolean flag=false;
			for (int i = 0; i < oneCoordinateVoList.size(); i++) {
				if (flag){
					//从右往左
					coordinateVos.add(twoCoordinateVoList.get(i));
					coordinateVos.add(oneCoordinateVoList.get(i));
					flag =false;
				}else {
					//从左往右
					coordinateVos.add(oneCoordinateVoList.get(i));
					coordinateVos.add(twoCoordinateVoList.get(i));
					flag = true;
				}
			}
		}
		
		return coordinateVos;
	}

}

CoordinateVo对象为：

@Data
public class CoordinateVo {
	private Double altitude;
	private Double latitude;
	private Double longitude;
}

使用方式：

此工具类主要有两个方法：

    defaultGenerateCoordinates：传入两个坐标点的经纬度，其余参数使用，默认值

    customGenerateCoordinates：传入两点，自定义的宽度、间隔、高度、长（短）边生成点

参数说明：

 lng1   经度,     lat1    纬度, 

 lng2    经度,     lat2    纬度,

 width  宽度,     interval 间距

 high  航点高度,direction  长短边 （true为长边方向，false为短边方向）

测试：

double long1=120.09675077317218;
		double lat1=30.24381935736484;
		double long2=120.09748061125909;
		double lat2=30.24408250042433;

		System.err.println("传入坐标A("+long1+","+lat1+")  坐标B("+long2+","+lat2+")");

		double distance = GeographicalCoordinatesUtils.getDistance(long1, lat1, long2, lat2);
		System.err.println("两点间距离-->"+distance);

		List<CoordinateVo> coordinateVos = GeographicalCoordinatesUtils.customGenerateCoordinates(
				long1, lat1, long2, lat2,10,10,100,false);

下面这些图为我前端测试的效果图，仅供大家参考