java 航线规划工具类
package com.xx.common.core.utils.geography;
import lombok.extern.slf4j.Slf4j;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
/**
* 航线规划工具类
*
* @author xie
* @date 2022-07-25
*/
@Slf4j
public class DrawWayPointUtil {
/**
* 地球平均半径（赤道半径）
*/
private static final double EARTH_RADIUS = 6378137;
/**
* 根据重叠率计算非重叠部分的真实距离公式（通用型）
*
* 航向重叠计算 拍摄第一张的位置与拍摄第二张的位置的距离差
* 已知短画幅=15.6mm，假设从无人机拿到的焦距是35mm，高度设置的是100米，航向重叠率设置80%，则无人机真正拍摄到的距离差为：
* double distance1 = getOverlapDistance(100,15.6,35,0.8);
* System.out.println("拍摄间距（航向间隔）：" + distance1);
*
* 旁向重叠计算 旁向重叠的计算影响航线的的间隔，对于一块固定地表，间隔越宽，则飞行的航线就越少，间隔越窄，则飞行的航线就越多，主要影响间隔的就是重叠率。
* 已知长画幅=23.5mm，假设从无人机拿到的焦距是35mm，高度设置的是100米，旁向重叠率设置70%，则航线间距离为:
* double distance2 = getOverlapDistance(100,23.5,35,0.7);
* System.out.println("航线间距离：" + distance2);
*
* @param height 无人机高度，单位米(m)
* @param frame 画幅（航向--短画幅、旁向--长画幅）传感器的长宽
* @param focal 焦距
* @param ratio 重叠率
* @return double 非重叠部分的真实距离
*/
public static double calcNonOverlapDistance(double height, double frame, double focal, double ratio) {
// TODO 如果focal焦距为0的话，则使用默认值值24毫米
focal = focal == 0 ? 25 : focal;
//单位换成米
focal = focal / 1000;
frame = frame / 1000;
// 设呈现的真实距离为x
double x;
// 拍摄到的距离
x = frame * height / focal;
// 设重叠距离
double d;
// 重叠部分的距离
d = ratio * x;
// 非重叠部分的距离 （单位米）
d = x - d;
return d;
}
/**
* 计算飞行速度
*
* @param courseInterval 航向间距
* @param timeInterval 时间间隔
* @return double 飞行拍摄速度
*/
public static double calcFlightSpeed(double courseInterval, double timeInterval) {
if (timeInterval == 0) {
return 0;
}
return courseInterval / timeInterval;
}
/**
* 计算两点距离
*/
public static double calculateLineDistance(LatLng latLng1, LatLng latLng2) {
double radLat1 = getRadian(latLng1.getLat());
double radLat2 = getRadian(latLng2.getLat());
// 两点纬度差
double latDiff = radLat1 - radLat2;
// 两点的经度差
double lngDiff = getRadian(latLng1.getLon()) - getRadian(latLng2.getLon());
double distance = 2 * Math.asin(Math.sqrt(Math.pow(Math.sin(latDiff / 2), 2) + Math.cos(radLat1) * Math.cos(radLat2) * Math.pow(Math.sin(lngDiff / 2), 2)));
distance = distance * EARTH_RADIUS;
return distance;
}
/**
* 角度弧度计算公式 rad:()
* 360度=2π π=Math.PI
* x度 = x*π/360 弧度
*/
public static double getRadian(double degree) {
return degree * Math.PI / 180.0;
}
/**
* 绘制航点航线
*
* @param cornerLatLngList 拐角点 LatLng 列表
* @param lineInterval 航线间隔
* @param routeAngle 航线角度
* @return java.util.List<LatLng>
*/
public static List<LatLng> drawFlyLines(List<LatLng> cornerLatLngList, double lineInterval, int routeAngle) {
log.info("开始绘制飞行航线");
// 组成外接矩形的5个点,用来辅助多边形绘制
List<LatLng> mBoundsEWSNLatLng = createPolygonBounds(cornerLatLngList);
// 多边形-边框
List<LatLng> rPolygons = createRotatePolygon(cornerLatLngList, mBoundsEWSNLatLng.get(0), -routeAngle);
// 组成外接矩形的5个点, 用来辅助航线绘制
List<LatLng> mRBoundsEWSNLatLng = createPolygonBounds(rPolygons);
Double[] latlines = createLats(mRBoundsEWSNLatLng, lineInterval);
// 对浮点数向上取整
int steps = (int) Math.ceil(latlines[0]);
List<LatLng> lines;
List<LatLng> polylineList = new ArrayList<>();
for (int i = 1; i < steps + 1; i++) {
lines = new ArrayList<>();
double fen = (latlines[1]) / 3 * 2;
for (int j = 0; j < rPolygons.size(); j++) {
int si = si(j + 1, rPolygons.size());
LatLng checklatlng =
createInlinePoint(rPolygons.get(j), rPolygons.get(si), mRBoundsEWSNLatLng.get(1).getLat() + fen - i * latlines[1]);
if (checklatlng != null) {
lines.add(checklatlng);
}
}
if (lines.size() < 2) {
continue;
}
if (lines.get(0) == lines.get(1)) {
continue;
}
if (i % 2 == 0) {
double min2 = Math.min(lines.get(0).getLon(), lines.get(1).getLon());
LatLng latLng1 = new LatLng(lines.get(0).getLat(), min2);
double max1 = Math.max(lines.get(0).getLon(), lines.get(1).getLon());
LatLng latLng2 = new LatLng(lines.get(0).getLat(), max1);
//
polylineList.add(latLng1);
polylineList.add(latLng2);
} else {
double max1 = Math.max(lines.get(0).getLon(), lines.get(1).getLon());
LatLng latLng1 = new LatLng(lines.get(0).getLat(), max1);
double min2 = Math.min(lines.get(0).getLon(), lines.get(1).getLon());
LatLng latLng2 = new LatLng(lines.get(0).getLat(), min2);
polylineList.add(latLng1);
polylineList.add(latLng2);
}
}
// 飞行航点集合
List<LatLng> mListPolylines = createRotatePolygon(polylineList, mBoundsEWSNLatLng.get(0), routeAngle);
return mListPolylines;
}
/**
* 无人机航线规划 ，基于凸多边形地块往复式运动。
* 凹多边形有时会出现不能完全覆盖的情况 。 一般处理方法就是将一个凹多边形切割成多个凸多边形 。
*
* @param cornerLatLngList 拐角点 LatLng 列表
* @param center 中心点
* @param rotate 角度
* @return java.util.List<LatLng>
*/
public static List<LatLng> createRotatePolygon(List<LatLng> cornerLatLngList, LatLng center, int rotate) {
List<LatLng> latLngList = new ArrayList<>();
for (LatLng latLng : cornerLatLngList) {
LatLng tr = transform(
latLng.getLon(),
latLng.getLat(),
center.getLon(),
center.getLat(),
rotate,
0,
0
);
latLngList.add(tr);
}
return latLngList;
}
/**
*
* @param x 当前节点经度
* @param y 当前节点纬度
* @param tx 中间节点经度
* @param ty 中间节点纬度
* @param deg 角度
* @param sx
* @param sy
* @return com.new3s.common.core.utils.geography.LatLng
*/
public static LatLng transform(double x, double y, double tx, double ty, int deg, int sx, int sy) {
Double[] doubles = new Double[2];
double sdeg = deg * Math.PI / 180;
if (sy == 0) {
sy = 1;
}
if (sx == 0) {
sx = 1;
}
doubles[0] = sx * ((x - tx) * Math.cos(sdeg) - (y - ty) * Math.sin(sdeg)) + tx;
doubles[1] = sy * ((x - tx) * Math.sin(sdeg) + (y - ty) * Math.cos(sdeg)) + ty;
LatLng latLng = new LatLng(doubles[1], doubles[0]);
return latLng;
}
/**
* 得到 中点 - 西北 - 东北 - 东南 - 西南 个点 - 外接矩形
* @param cornerLatLngList 拐角点 LatLng 列表
* @return java.util.List<LatLng> 绘制出多边形的外接矩形四顶点、外接矩形的中心点
*/
public static List<LatLng> createPolygonBounds(List<LatLng> cornerLatLngList) {
List<LatLng> latLngSparseArray = new ArrayList<>();
if (cornerLatLngList.size() < 1) {
return latLngSparseArray;
}
// 纬度
List<Double> latsLists = new ArrayList<>();
// 经度
List<Double> lngsLists = new ArrayList<>();
for (LatLng latLng : cornerLatLngList) {
latsLists.add(latLng.getLat());
lngsLists.add(latLng.getLon());
}
// 最东经
double lngsMax = Collections.max(lngsLists);
// 最北纬
double latsMax = Collections.max(latsLists);
// 最南纬
double latsMin = Collections.min(latsLists);
// 最西经
double lngsMin = Collections.min(lngsLists);
double lngsCenter = (lngsMax + lngsMin) / 2;
double latsCenter = (latsMax + latsMin) / 2;
// 0 中点
latLngSparseArray.add(new LatLng(latsCenter, lngsCenter));
// 1 西北
latLngSparseArray.add(new LatLng(latsMax, lngsMin));
// 2 东北
latLngSparseArray.add(new LatLng(latsMax, lngsMax));
// 3 东南
latLngSparseArray.add(new LatLng(latsMin, lngsMax));
// 4 西南
latLngSparseArray.add(new LatLng(latsMin, lngsMin));
return latLngSparseArray;
}
/**
* 计算有多少条纬度线穿过 纬度线相差lat
* @param mRBoundsEWSNLatLng 组成外接矩形的5个点
* @param lineInterval 航线间隔
* @return java.lang.Double[]
*/
public static Double[] createLats(List<LatLng> mRBoundsEWSNLatLng, double lineInterval) {
Double[] integers = new Double[2];
//线条数量
double steps = (calculateLineDistance(mRBoundsEWSNLatLng.get(1), mRBoundsEWSNLatLng.get(4)) / lineInterval);
//纬度差
double lat = (mRBoundsEWSNLatLng.get(1).getLat() - mRBoundsEWSNLatLng.get(4).getLat()) / steps;
integers[0] = steps;
integers[1] = lat;
return integers;
}
public static int si(int i, int len) {
if (i > len - 1) {
return i - len;
}
if (i < 0) {
return len + i;
}
return i;
}
/**
* 计算纬度线 与边缘线的交点
* @param latLng1
* @param latLng2
* @param y
* @return LatLng
*/
public static LatLng createInlinePoint(LatLng latLng1, LatLng latLng2, double y) {
LatLng latLng;
double s = latLng1.getLat() - latLng2.getLat();
double x;
if (s > 0 || s < 0) {
x = (y - latLng1.getLat()) * (latLng1.getLon() - latLng2.getLon()) / s + latLng1.getLon();
} else {
return null;
}
// 判断x是否在p1,p2在x轴的投影里，不是的话返回null
if (x > latLng1.getLon() && x > latLng2.getLon()) {
return null;
}
if (x < latLng1.getLon() && x < latLng2.getLon()) {
return null;
}
latLng = new LatLng(y, x);
return latLng;
}
public static void main(String[] args) {
//===========================航向航线间距===========================
// 航向重叠计算 拍摄第一张的位置与拍摄第二张的位置的距离差
// 已知短画幅=15.6mm，假设从无人机拿到的焦距是35mm，高度设置的是100米，航向重叠率设置80%，则无人机真正拍摄到的距离差为：
double courseInterval = calcNonOverlapDistance(100,15.6,25,0.8);
System.out.println("拍摄间距（航向间隔）：" + courseInterval);
// 速度 距离=速度*时间 拍照的时间间隔是固定的 假设我们设计的时间是每隔2s进行一次拍照
double flightSpeed = calcFlightSpeed(courseInterval, 2);
System.out.println("拍摄速度：" + flightSpeed);
// 旁向重叠计算 旁向重叠的计算影响航线的的间隔，对于一块固定地表，间隔越宽，则飞行的航线就越少，间隔越窄，则飞行的航线就越多，主要影响间隔的就是重叠率。
// 已知长画幅=23.5mm，假设从无人机拿到的焦距是35mm，高度设置的是100米，旁向重叠率设置70%，则航线间距离为:
double lineInterval = calcNonOverlapDistance(100,23.5,25,0.7);
System.out.println("航线间距离：" + lineInterval);
System.out.println("\n\n");
//===========================两点之间的距离===========================
LatLng latLngA = new LatLng(39.926954548891196, 116.39586567878723);
LatLng latLngB = new LatLng(39.92582530825492, 116.39715984463693);
double distance = calculateLineDistance(latLngA, latLngB);
System.out.println("两点之间的距离1 " + distance);
distance = (double) Math.round(distance * 10) / 10;
System.out.println("两点之间的距离2 " + distance);
//===========================绘制飞行航线===========================
LatLng latLng1 = new LatLng(39.926954548891196, 116.39586567878723);
LatLng latLng2 = new LatLng(39.92582530825492, 116.39715984463693);
LatLng latLng3 = new LatLng(39.92453663475451, 116.3964705169201);
LatLng latLng4 = new LatLng(39.92476907087396, 116.39957115054132);
LatLng latLng5 = new LatLng(39.927058421639025, 116.39960333704948);
List<LatLng> cornerLatLngList = Arrays.asList(latLng1, latLng2, latLng3, latLng4, latLng5);
List<LatLng> drawFlyLines = drawFlyLines(cornerLatLngList, 32, 137);
System.out.println("航点规划-----------" + drawFlyLines.size());
drawFlyLines.forEach(item -> System.out.println(item.getLat() + " " + item.getLon()));
}
}
欢迎一起来学习和指导，谢谢关注！
本文来自博客园，作者：xiexie0812，转载请注明原文链接：https://www.cnblogs.com/mask-xiexie/p/16877751.html
