package com.dji.sample.wayline.plane;
|
|
import com.dji.sample.patches.utils.GeoToolsUtil;
|
import com.dji.sample.wayline.plane.param.CreateWaylineParam;
|
import lombok.extern.slf4j.Slf4j;
|
import org.geotools.geometry.jts.JTSFactoryFinder;
|
import org.locationtech.jts.geom.*;
|
|
import java.util.ArrayList;
|
import java.util.List;
|
|
/**
|
* @Author AIX
|
* @Date 2024/7/9 21:33
|
* 面状航线
|
* @Version 1.0
|
*/
|
@Slf4j
|
public class PlaneCourseUtils {
|
|
/**
|
* @param height 无人机高度，单位米(m)
|
* @param frame 画幅
|
* @param focal 焦距
|
* @param ratio 重叠率
|
*/
|
private static double get(double height, double frame, double focal, double ratio) {
|
//todo 如果focal焦距为0的话，则使用默认值值24毫米
|
focal = focal == 0 ? 24 : 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 latLng 经纬度
|
* @param distance 距离
|
* @param bearing 方位角
|
* @return
|
*/
|
private static MapLatLng getBearingLatLng(MapLatLng latLng, int distance, double bearing) {
|
|
double R = 6371.393 * 1000;
|
|
double δ = distance / R;
|
double θ = toRadius(bearing);
|
|
double φ1 = toRadius(latLng.getLatitude());
|
double λ1 = toRadius(latLng.getLongitude());
|
|
double sinφ2 = Math.sin(φ1) * Math.cos(δ) + Math.cos(φ1) * Math.sin(δ) * Math.cos(θ);
|
double φ2 = Math.asin(sinφ2);
|
|
double y = Math.sin(θ) * Math.sin(δ) * Math.cos(φ1);
|
double x = Math.cos(δ) - Math.sin(φ1) * sinφ2;
|
double λ2 = λ1 + Math.atan2(y, x);
|
|
double lat = toDegree(φ2);
|
double lng = toDegree(λ2);
|
|
return new MapLatLng(lat, lng);
|
}
|
|
private static double toRadius(double value) {
|
return value * Math.PI / 180;
|
}
|
|
private static double toDegree(double value) {
|
return value * 180 / Math.PI;
|
}
|
|
/**
|
* 最北纬点的经纬度
|
*
|
* @param polygon 面集合
|
* @return
|
*/
|
private static MapLatLng maxLatPoint(List<double[]> polygon) {
|
// 查找最北的点
|
double maxLat = Double.MIN_VALUE;
|
double[] maxLatPoint = null;
|
|
for (double[] point : polygon) {
|
if (point[1] > maxLat) {
|
maxLat = point[1];
|
maxLatPoint = point;
|
}
|
}
|
return new MapLatLng(maxLatPoint[1], maxLatPoint[0]);
|
}
|
|
/**
|
* 最南纬点的经纬度
|
*
|
* @param polygon 面集合
|
* @return
|
*/
|
private static MapLatLng minLatPoint(List<double[]> polygon) {
|
// 查最南的点
|
double minLat = Double.MAX_VALUE;
|
double[] minLatPoint = null;
|
|
for (double[] point : polygon) {
|
if (point[1] < minLat) {
|
minLat = point[1];
|
minLatPoint = point;
|
}
|
}
|
return new MapLatLng(minLatPoint[1], minLatPoint[0]);
|
}
|
|
/**
|
* 根据左上角经纬度生成面点集合
|
*
|
* @param mapLatLng 从哪个点开始
|
* @param intValue 距离
|
* @param bearing 角度
|
* @param mapLatLngMin 面最南经纬度
|
*/
|
private static List<MapLatLng> createPoint(MapLatLng mapLatLng, int intValue, double bearing, MapLatLng mapLatLngMin) {
|
|
List<MapLatLng> pointList = new ArrayList<>();
|
int index = 0;//防止死循环
|
while (true) {
|
if (index > 100)
|
break ;
|
MapLatLng newLocation = getBearingLatLng(mapLatLng, intValue, bearing);
|
if (newLocation.getLatitude() <= mapLatLngMin.getLatitude())
|
break;
|
|
mapLatLng = newLocation;
|
pointList.add(newLocation);
|
index += 1;
|
log.info("创建航点，当前index:{}", index);
|
|
}
|
return pointList;
|
}
|
|
public static List<Coordinate> createWaylinePoints(CreateWaylineParam param) {
|
// 获取外接矩形
|
BoundingRectangle boundingRectangle = BoundingRectangle.fromPolygon(param.getPolygon());
|
|
// 航向重叠计算 待完成
|
|
// 旁向重叠计算
|
double distance2 = PlaneCourseUtils.get(param.getHeight(), param.getFrame(), param.getFocal(), param.getSideRatio());
|
System.out.println("旁向重叠计算距离:" + distance2 * 100 / 100);
|
int intValue = (int) distance2;
|
|
// 以最北纬度为基准点，在航线间隔距离下，
|
// 计算出 180 度方向(正北为0度)的一个点，该点则是第一条航线上的点，
|
// 然后再通过该点为基准点，一直反复计算出下一个点，直到下一个航点的位置小于最南纬度为止
|
|
//最南纬点经纬度
|
MapLatLng mapLatLngMin = PlaneCourseUtils.minLatPoint(param.getPolygon());
|
|
List<MapLatLng> topLeftPointList = PlaneCourseUtils.createPoint(boundingRectangle.topLeft, intValue, param.getBearing() * 2, mapLatLngMin);
|
List<MapLatLng> topRightPointList = PlaneCourseUtils.createPoint(boundingRectangle.topRight, intValue, param.getBearing() * 2, mapLatLngMin);
|
|
List<LineString> lineStrings = PlaneCourseUtils.createLine(topLeftPointList, topRightPointList);
|
|
List<Coordinate> points = PlaneCourseUtils.getPoints(param.getPolygon(), lineStrings);
|
|
return points;
|
}
|
|
//MULTIPOLYGON(((115.86528871951153 28.625287925196325,
|
// 115.86561708513025 28.625787612007546,
|
// 115.86815834948467 28.624602640426623,
|
// 115.86773004650362 28.62425999804172,
|
// 115.86528871951153 28.625287925196325)))
|
public static void main(String[] args) {
|
|
//外接矩形最北经纬度和最南经纬度
|
List<double[]> polygon = new ArrayList<>();
|
double[] a = {116.028037250229,25.8948290570725};
|
double[] a2 = {116.031565260299,25.8950672687002};
|
double[] a3 = {116.032112214864,25.8901339309971};
|
double[] a4 = {116.028726270654,25.8895106128711};
|
polygon.add(a);
|
polygon.add(a2);
|
polygon.add(a3);
|
polygon.add(a4);
|
BoundingRectangle boundingRectangle = BoundingRectangle.fromPolygon(polygon);
|
System.out.println("northLatLng:" + boundingRectangle);
|
|
//最北纬度点
|
MapLatLng mapLatLngMax = maxLatPoint(polygon);
|
MapLatLng mapLatLngMin = minLatPoint(polygon);
|
System.out.println("最北纬度点：" + mapLatLngMax);
|
System.out.println("最南纬度点：" + mapLatLngMin);
|
|
// 航向重叠计算
|
//已知短画幅=24mm，假设从无人机拿到的焦距是26mm，高度设置的是100米，重叠率设置80%，则无人机真正拍摄到的距离差为：
|
// m30t:5.0125 焦距：21-75 mm（等效焦距：113-405 mm）
|
// m3d:6.9999943 等效焦距：24 mm
|
double distance1 = get(100, 24, 26, 0.8);
|
System.out.println("航向重叠计算距离" + distance1);
|
|
// 旁向重叠计算
|
//旁向重叠的计算影响航线的的间隔，对于一块固定地表，间隔越宽，则飞行的航线就越少，
|
// 间隔越窄，则飞行的航线就越多，主要影响间隔的就是重叠率。
|
// 已知长画幅=35mm，假设从无人机拿到的焦距是26mm，高度设置的是100米，重叠率设置80%，则航线间距离为:
|
// 为什么是26mm 对焦距离 = 5 + 最小焦距 = 21
|
double distance2 = get(372.3, 100, 10*6.9999943, 0.8);
|
System.out.println("旁向重叠计算距离:" + distance2 * 100 / 100);
|
int intValue = (int) distance2;
|
|
// 创建一个起始的MapLatLng对象 以最北纬度为基准点
|
// MapLatLng startLocation = new MapLatLng(boundingRectangle.bottomLeft.getLatitude(), boundingRectangle.bottomLeft.getLongitude()); // 经纬度
|
//
|
// 定义要移动的距离（单位：米）和方位角（单位：度）
|
double bearing = 180; // 方向
|
|
// 调用getBearingLatLng方法计算新位置
|
System.out.println("第一条线路：" + mapLatLngMax);
|
System.out.println("------------------------------------------------");
|
|
// 以最北纬度为基准点，在航线间隔距离下，
|
// 计算出 180 度方向(正北为0度)的一个点，该点则是第一条航线上的点，
|
// 然后再通过该点为基准点，一直反复计算出下一个点，直到下一个航点的位置小于最南纬度为止
|
List<MapLatLng> topLeftPointList = createPoint(boundingRectangle.topLeft, intValue, bearing, mapLatLngMin);
|
List<MapLatLng> topRightPointList = createPoint(boundingRectangle.topRight, intValue, bearing, mapLatLngMin);
|
|
List<LineString> lineStrings = createLine(topLeftPointList, topRightPointList);
|
|
List<Coordinate> points = getPoints(polygon, lineStrings);
|
for (Coordinate point:points) {
|
System.out.println("" + point.x + ", " + point.y + ";");
|
}
|
|
}
|
|
/**
|
* 创建线的集合
|
*
|
* @param topLeftPointList 左上角点集合
|
* @param topRightPointList 右上角点集合
|
*/
|
private static List<LineString> createLine(List<MapLatLng> topLeftPointList, List<MapLatLng> topRightPointList) {
|
// 创建GeometryFactory
|
GeometryFactory geometryFactory = JTSFactoryFinder.getGeometryFactory();
|
|
//线的集合
|
List<LineString> lineStrings = new ArrayList<>();
|
// 定义两点来创建LineString
|
for (int i = 0; i < topLeftPointList.size(); i++) {
|
MapLatLng topLeft = topLeftPointList.get(i);
|
MapLatLng topRight = topRightPointList.get(i);
|
Coordinate start = new Coordinate(topLeft.getLongitude(), topRight.getLatitude(), 0);
|
Coordinate end = new Coordinate(topRight.getLongitude(), topRight.getLatitude(), 0);
|
LineString line = geometryFactory.createLineString(new Coordinate[]{start, end});
|
lineStrings.add(line);
|
System.out.println("line:" + line);
|
}
|
return lineStrings;
|
}
|
|
/**
|
* 获取面和线的交集点集合
|
*
|
* @param polygonNums
|
* @param lineStrings
|
*/
|
private static List<Coordinate> getPoints(List<double[]> polygonNums, List<LineString> lineStrings) {
|
|
// 创建GeometryFactory
|
GeometryFactory geometryFactory = JTSFactoryFinder.getGeometryFactory();
|
|
// 检查polygonNums是否为空或长度小于3（至少需要三个点才能形成一个闭合多边形）
|
if (polygonNums == null || polygonNums.size() < 3) {
|
System.out.println("多边形顶点数量不足或为空");
|
return null;
|
}
|
|
// 创建坐标数组并闭合多边形
|
Coordinate[] shell = new Coordinate[polygonNums.size() + 1];
|
for (int i = 0; i < polygonNums.size(); i++) {
|
double[] polygonD = polygonNums.get(i);
|
shell[i] = new Coordinate(polygonD[0], polygonD[1], 0);
|
}
|
// 闭合多边形
|
shell[polygonNums.size()] = new Coordinate(polygonNums.get(0)[0], polygonNums.get(0)[1], 0);
|
|
// 创建多边形
|
Polygon polygon = geometryFactory.createPolygon(shell);
|
|
System.out.println("polygon:" + polygon);
|
|
Coordinate centro = GeoToolsUtil.getCentro(polygon);
|
|
//返回集合
|
List<Coordinate> retPoint = new ArrayList<>();
|
|
// 循环处理线集合
|
int index = 0;
|
for (LineString line : lineStrings) {
|
index = index + 1;
|
// 计算交集
|
Geometry intersection = polygon.intersection(line);
|
|
// 处理交集结果
|
if (intersection instanceof Point) {
|
System.out.println("交点坐标: " + intersection.getCoordinate());
|
} else if (intersection instanceof MultiPoint) {
|
// 如果有多个交点
|
for (int i = 0; i < intersection.getNumGeometries(); i++) {
|
Point point = (Point) intersection.getGeometryN(i);
|
retPoint.add(point.getCoordinate());
|
}
|
} else if (intersection instanceof LineString) {
|
// 交集是一条线段，但这里我们可能不直接打印它作为点
|
System.out.println("交集是一个LineString。");
|
// 如果需要，可以打印线段的起点和终点
|
LineString lineString = (LineString) intersection;
|
Coordinate start = lineString.getStartPoint().getCoordinate();
|
Coordinate end = lineString.getEndPoint().getCoordinate();
|
if (!(index%2==0)) {
|
retPoint.add(start);
|
retPoint.add(end);
|
} else {
|
retPoint.add(end);
|
retPoint.add(start);
|
}
|
|
} else {
|
// 如果没有交点或结果不是点
|
System.out.println("没有交点或未知类型");
|
}
|
}
|
retPoint.add(centro);
|
return retPoint;
|
}
|
|
}
|
