基于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);
下面这些图为我前端测试的效果图，仅供大家参考
2172
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
