forked from uav/ard-uav-server
ard-system/src/main/java/com/ard/data/noflyzone/utils/NoFlyZoneUtils.java
@@ -2,104 +2,250 @@ import com.ard.data.noflyzone.domain.ArdNoFlyBox; import com.ard.data.noflyzone.domain.ArdNoFlyZone; import com.ard.data.noflyzone.domain.FeatureCollection; import com.ard.data.noflyzone.domain.Vertex; import org.locationtech.jts.geom.Coordinate; import org.locationtech.jts.geom.GeometryFactory; import org.locationtech.jts.geom.LineString; import org.locationtech.jts.geom.Polygon; import java.util.ArrayList; import java.util.Arrays; import java.util.List; public class NoFlyZoneUtils { /** * 判断点是否在禁飞区集合中 * * @param point 目标点 * @param zones 禁飞区集合 * @return true: 点在任意禁飞区内 * 计算点到多边形的最近距离 */ public static boolean isInNoFlyZones(Vertex point, List<ArdNoFlyZone> zones) { if (point == null || zones == null || zones.isEmpty()) { return false; public static double pointToPolygonDistance(Vertex point, List<Vertex> polygon) { // 如果在多边形内部，距离为 0 if (isPointInPolygon(point, polygon)) { return 0; } for (ArdNoFlyZone zone : zones) { // 1️⃣ 先快速判断外接矩形 if (zone.getArdNoFlyBox() != null && !isInBox(point, zone.getArdNoFlyBox())) { continue; // 点不在外接矩形内，跳过 } double minDistance = Double.MAX_VALUE; int n = polygon.size(); // 2️⃣ 多边形判断 List<Vertex> vertices = zone.getVertices(); if (vertices != null && !vertices.isEmpty()) { if (isPointInPolygon(point, vertices)) { return true; } for (int i = 0; i < n; i++) { Vertex a = polygon.get(i); Vertex b = polygon.get((i + 1) % n); // 下一点，最后一条边连接第一个点 double dist = pointToSegmentDistance(point, a, b); minDistance = Math.min(minDistance, dist); } return minDistance; } /** * 计算点到线段的距离 */ public static double pointToSegmentDistance(Vertex p, Vertex a, Vertex b) { double x = p.x, y = p.y; double x1 = a.x, y1 = a.y; double x2 = b.x, y2 = b.y; double dx = x2 - x1; double dy = y2 - y1; if (dx == 0 && dy == 0) { // A 和 B 是同一个点 dx = x - x1; dy = y - y1; return Math.sqrt(dx * dx + dy * dy); } // 投影参数 t（0-1之间表示在线段内部） double t = ((x - x1) * dx + (y - y1) * dy) / (dx * dx + dy * dy); if (t < 0) { // 最近点是 A dx = x - x1; dy = y - y1; } else if (t > 1) { // 最近点是 B dx = x - x2; dy = y - y2; } else { // 最近点在线段中间 double projX = x1 + t * dx; double projY = y1 + t * dy; dx = x - projX; dy = y - projY; } return Math.sqrt(dx * dx + dy * dy); } /** * 判断点是否在多边形内（射线法） */ public static boolean isPointInPolygon(Vertex point, List<Vertex> polygon) { int n = polygon.size(); boolean inside = false; for (int i = 0, j = n - 1; i < n; j = i++) { Vertex vi = polygon.get(i); Vertex vj = polygon.get(j); boolean intersect = ((vi.y > point.y) != (vj.y > point.y)) && (point.x < (vj.x - vi.x) * (point.y - vi.y) / (vj.y - vi.y) + vi.x); if (intersect) inside = !inside; } return inside; } /** * 判断线段是否与多边形相交 */ public static boolean isLineIntersectPolygon(Vertex p1, Vertex p2, List<Vertex> polygon) { int n = polygon.size(); for (int i = 0; i < n; i++) { Vertex a = polygon.get(i); Vertex b = polygon.get((i + 1) % n); if (isSegmentsIntersect(p1, p2, a, b)) { return true; } } return false; } /** * 判断点是否在矩形框内 * 判断两条线段是否相交 */ private static boolean isInBox(Vertex point, ArdNoFlyBox box) { return point.x >= box.getMinx() && point.x <= box.getMaxx() && point.y >= box.getMiny() && point.y <= box.getMaxy(); } private static boolean isSegmentsIntersect(Vertex p1, Vertex p2, Vertex p3, Vertex p4) { double d1 = cross(p3, p4, p1); double d2 = cross(p3, p4, p2); double d3 = cross(p1, p2, p3); double d4 = cross(p1, p2, p4); /** * 射线法判断点是否在多边形内 */ private static boolean isPointInPolygon(Vertex point, List<Vertex> vertices) { if (point == null || vertices == null || vertices.size() < 3) { return false; if (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) && ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) { return true; } int count = 0; int n = vertices.size(); double epsilon = 1e-9; // 处理共线情况 if (d1 == 0 && onSegment(p3, p4, p1)) return true; if (d2 == 0 && onSegment(p3, p4, p2)) return true; if (d3 == 0 && onSegment(p1, p2, p3)) return true; if (d4 == 0 && onSegment(p1, p2, p4)) return true; for (int i = 0; i < n; i++) { Vertex v1 = vertices.get(i); Vertex v2 = vertices.get((i + 1) % n); return false; } // 点就是顶点 if (v1.IsSame(point)) { return true; } private static double cross(Vertex p1, Vertex p2, Vertex p3) { return (p2.x - p1.x) * (p3.y - p1.y) - (p3.x - p1.x) * (p2.y - p1.y); } double minX = Math.min(v1.x, v2.x); double maxX = Math.max(v1.x, v2.x); double minY = Math.min(v1.y, v2.y); double maxY = Math.max(v1.y, v2.y); private static boolean onSegment(Vertex p1, Vertex p2, Vertex q) { return q.x <= Math.max(p1.x, p2.x) && q.x >= Math.min(p1.x, p2.x) && q.y <= Math.max(p1.y, p2.y) && q.y >= Math.min(p1.y, p2.y); } // 水平线段 if (Math.abs(minY - maxY) < epsilon) { if (Math.abs(point.y - minY) < epsilon && point.x >= minX && point.x <= maxX) { return true; } else { continue; } } // 点不在纵向范围 if (point.y < minY || point.y > maxY) { /** * 判断点是否在矩形内 */ public static boolean isInsideBox(Vertex point, ArdNoFlyBox box) { if (point == null || box == null) return false; return point.x >= box.getMinx() && point.x <= box.getMaxx() && point.y >= box.getMiny() && point.y <= box.getMaxy(); } /** * 根据 GeoJSON（FeatureCollection）计算外接矩形 */ @SuppressWarnings("unchecked") public static ArdNoFlyBox getArdNoFlyBox(FeatureCollection featureCollection) { if (featureCollection == null || featureCollection.getFeatures() == null) { return null; } double minx = Double.MAX_VALUE; double miny = Double.MAX_VALUE; double maxx = Double.MIN_VALUE; double maxy = Double.MIN_VALUE; for (FeatureCollection.Feature feature : featureCollection.getFeatures()) { FeatureCollection.Geometry geometry = feature.getGeometry(); if (geometry == null || geometry.getCoordinates() == null) { continue; } double xIntersect = v1.x + (point.y - v1.y) * (v2.x - v1.x) / (v2.y - v1.y); String type = geometry.getType(); Object coords = geometry.getCoordinates(); if (Math.abs(xIntersect - point.x) < epsilon) { return true; // 点在边界 if ("Polygon".equalsIgnoreCase(type)) { // Polygon: [[[lon, lat], [lon, lat], ...]] List<List<List<Double>>> rings = (List<List<List<Double>>>) coords; for (List<List<Double>> ring : rings) { for (List<Double> point : ring) { if (point.size() < 2) { continue; } double x = point.get(0); double y = point.get(1); minx = Math.min(minx, x); miny = Math.min(miny, y); maxx = Math.max(maxx, x); maxy = Math.max(maxy, y); } } } if (xIntersect > point.x) { count++; } if (Math.abs(point.y - minY) < epsilon) { count--; else if ("MultiPolygon".equalsIgnoreCase(type)) { // MultiPolygon: [[[[lon, lat], ...]], ...] List<List<List<List<Double>>>> polygons = (List<List<List<List<Double>>>>) coords; for (List<List<List<Double>>> polygon : polygons) { for (List<List<Double>> ring : polygon) { for (List<Double> point : ring) { if (point.size() < 2) { continue; } double x = point.get(0); double y = point.get(1); minx = Math.min(minx, x); miny = Math.min(miny, y); maxx = Math.max(maxx, x); maxy = Math.max(maxy, y); } } } } } return count % 2 != 0; return new ArdNoFlyBox(minx, miny, maxx, maxy); } /** * 将 GeoJSON Polygon 的外环坐标转换成 Vertex 列表 */ public static List<Vertex> toVertexes(FeatureCollection featureCollection) { List<Vertex> vertexes = new ArrayList<>(); if (featureCollection == null || featureCollection.getFeatures() == null || featureCollection.getFeatures().isEmpty()) { return vertexes; } FeatureCollection.Feature feature = featureCollection.getFeatures().get(0); if (feature.getGeometry() != null) { List<List<List<Double>>> coords = feature.getGeometry().getCoordinates(); if (coords != null && !coords.isEmpty()) { List<List<Double>> outerRing = coords.get(0); for (List<Double> point : outerRing) { if (point.size() >= 2) { vertexes.add(new Vertex(point.get(0), point.get(1))); } } } } return vertexes; } public static double degDistanceToMeter(double deg, double lat) { // 1° ≈ 111320 米，但是经度和纬度不一样，这里用纬度做修正 return deg * 111320.0 * Math.cos(Math.toRadians(lat)); } }
