GeCaoAPP.git - Gitblit

			@@ -1,14 +1,19 @@
			package lujing;

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

			/**
			* 无障碍物凸形草地路径规划类 (优化版)
			* 无障碍物凸形草地路径规划类 (终极优化版)
			* 特性：
			* 1. 最小投影宽度方向选择 (效率最高，转弯最少)
			* 2. 边缘轮廓优先切割 (无死角覆盖)
			* 3. 支持外部传入已包含重叠率的宽度参数
			*/
			public class AoxinglujingNoObstacle {

			// 优化：引入极小值用于浮点数比较，处理几何精度误差
			// 引入极小值用于浮点数比较，处理几何精度误差
			private static final double EPSILON = 1e-6;

			/**
			@@ -49,15 +54,15 @@
			}

			/**
			* 对外公开的静态调用方法 (保留字符串入参格式)
			* 对外公开的静态调用方法
			*
			* @param boundaryCoordsStr 地块边界坐标字符串 "x1,y1;x2,y2;..."
			* @param mowingWidthStr 割草宽度字符串，如 "0.34"
			* @param mowingWidthStr 有效割草宽度字符串 (已包含重叠率)，如 "0.30"
			* @param safetyMarginStr 安全边距字符串，如 "0.2"
			* @return 路径段列表
			*/
			public static List<PathSegment> planPath(String boundaryCoordsStr, String mowingWidthStr, String safetyMarginStr) {
			// 1. 解析参数 (优化：单独提取解析逻辑)
			// 1. 解析参数
			List<Point> originalPolygon = parseCoords(boundaryCoordsStr);
			double mowingWidth;
			double safetyMargin;
			@@ -74,85 +79,119 @@
			}

			/**
			* 核心算法逻辑 (强类型入参，便于测试和内部调用)
			* 核心算法逻辑
			*/
			private static List<PathSegment> planPathCore(List<Point> originalPolygon, double mowingWidth, double safetyMargin) {
			// 优化：三角形也是合法的凸多边形，限制改为小于3
			private static List<PathSegment> planPathCore(List<Point> originalPolygon, double width, double safetyMargin) {
			if (originalPolygon == null \|\| originalPolygon.size() < 3) {
			throw new IllegalArgumentException("多边形坐标点不足3个，无法构成有效区域");
			return new ArrayList<>(); // 或抛出异常，视业务需求而定
			}

			// 确保多边形是逆时针方向
			ensureCCW(originalPolygon);

			// 1. 根据安全边距内缩
			List<Point> shrunkPolygon = shrinkPolygon(originalPolygon, safetyMargin);
			// 1. 根据安全边距内缩，得到实际作业区域
			List<Point> workAreaPolygon = shrinkPolygon(originalPolygon, safetyMargin);

			// 优化：内缩后如果多边形失效（例如地块太窄，内缩后消失），直接返回空路径，避免后续报错
			if (shrunkPolygon.size() < 3) {
			// 这里可以记录日志：地块过小，无法满足安全距离作业
			// 如果内缩后区域失效（如地块太小），返回空路径
			if (workAreaPolygon.size() < 3) {
			return new ArrayList<>();
			}

			// 2. 计算最长边角度 (作为扫描方向)
			double angle = calculateLongestEdgeAngle(originalPolygon);
			List<PathSegment> finalPath = new ArrayList<>();

			// 3. & 4. 旋转扫描并剪裁
			List<PathSegment> mowingLines = generateClippedMowingLines(shrunkPolygon, angle, mowingWidth);
			// 2. [优化] 优先生成轮廓路径 (Contour Pass)
			// 沿作业边界走一圈，确保边缘整齐且无遗漏
			addContourPath(workAreaPolygon, finalPath);

			// 5. 弓字形连接
			return connectPathSegments(mowingLines);
			}
			// 3. [优化] 计算最佳扫描角度
			// 寻找让多边形投影高度最小的角度，从而最小化转弯次数
			double bestAngle = findOptimalScanAngle(workAreaPolygon);

			// ================= 核心算法辅助方法 =================
			// 4. 生成内部弓字形路径
			// 直接使用传入的 width (已包含重叠率)
			List<PathSegment> zigZagPaths = generateClippedMowingLines(workAreaPolygon, bestAngle, width);

			private static List<Point> shrinkPolygon(List<Point> polygon, double margin) {
			List<Point> newPoints = new ArrayList<>();
			int n = polygon.size();

			for (int i = 0; i < n; i++) {
			Point p1 = polygon.get(i);
			Point p2 = polygon.get((i + 1) % n);
			Point p0 = polygon.get((i - 1 + n) % n);

			Line line1 = offsetLine(p1, p2, margin);
			Line line0 = offsetLine(p0, p1, margin);

			Point intersection = getIntersection(line0, line1);
			// 5. 连接轮廓路径和弓字形路径
			if (!finalPath.isEmpty() && !zigZagPaths.isEmpty()) {
			Point contourEnd = finalPath.get(finalPath.size() - 1).end;
			Point zigzagStart = zigZagPaths.get(0).start;

			// 优化：增加非空判断，且如果交点异常远（尖角效应），实际工程中可能需要切角处理
			// 这里暂保留基础逻辑，但在凸多边形中通常没问题
			if (intersection != null) {
			newPoints.add(intersection);
			// 如果轮廓终点与弓字形起点不重合，添加过渡段
			if (distanceSq(contourEnd, zigzagStart) > EPSILON) {
			finalPath.add(new PathSegment(contourEnd, zigzagStart, false));
			}
			}
			return newPoints;

			// 6. 合并弓字形路径
			finalPath.addAll(connectPathSegments(zigZagPaths));

			return finalPath;
			}

			private static double calculateLongestEdgeAngle(List<Point> polygon) {
			double maxDistSq = -1;
			double angle = 0;
			int n = polygon.size();
			// ================= 核心逻辑辅助方法 =================

			/**
			* 添加轮廓路径 (围着多边形走一圈)
			*/
			private static void addContourPath(List<Point> polygon, List<PathSegment> path) {
			int n = polygon.size();
			for (int i = 0; i < n; i++) {
			Point p1 = polygon.get(i);
			Point p2 = polygon.get((i + 1) % n);
			double dx = p2.x - p1.x;
			double dy = p2.y - p1.y;
			double distSq = dx * dx + dy * dy;
			path.add(new PathSegment(p1, p2, true));
			}
			}

			if (distSq > maxDistSq) {
			maxDistSq = distSq;
			angle = Math.atan2(dy, dx);
			/**
			* 寻找最优扫描角度 (最小投影高度法)
			*/
			private static double findOptimalScanAngle(List<Point> polygon) {
			double minHeight = Double.MAX_VALUE;
			double bestAngle = 0;
			int n = polygon.size();

			// 遍历每一条边，计算以该边为“底”时，多边形的高度
			for (int i = 0; i < n; i++) {
			Point p1 = polygon.get(i);
			Point p2 = polygon.get((i + 1) % n);

			// 当前边的角度
			double currentAngle = Math.atan2(p2.y - p1.y, p2.x - p1.x);

			// 计算在这个角度下的投影高度
			double height = calculatePolygonHeightAtAngle(polygon, currentAngle);

			if (height < minHeight) {
			minHeight = height;
			bestAngle = currentAngle;
			}
			}
			return angle;
			return bestAngle;
			}

			/**
			* 计算多边形在特定旋转角度下的Y轴投影高度
			*/
			private static double calculatePolygonHeightAtAngle(List<Point> poly, double angle) {
			double minY = Double.MAX_VALUE;
			double maxY = -Double.MAX_VALUE;

			double cos = Math.cos(-angle);
			double sin = Math.sin(-angle);

			for (Point p : poly) {
			// 只需计算旋转后的Y坐标
			double rotatedY = p.x * sin + p.y * cos;
			if (rotatedY < minY) minY = rotatedY;
			if (rotatedY > maxY) maxY = rotatedY;
			}
			return maxY - minY;
			}

			private static List<PathSegment> generateClippedMowingLines(List<Point> polygon, double angle, double width) {
			List<PathSegment> segments = new ArrayList<>();

			// 旋转至水平

			// 旋转多边形至水平
			List<Point> rotatedPoly = rotatePolygon(polygon, -angle);

			double minY = Double.MAX_VALUE;
			@@ -162,28 +201,27 @@
			if (p.y > maxY) maxY = p.y;
			}

			// 优化：起始扫描线增加一个微小的偏移量 EPSILON
			// 避免扫描线正好落在顶点上，导致交点判断逻辑出现二义性
			// 起始扫描线位置：
			// 从 minY + width/2 开始，因为之前已经走了轮廓线(Contour Pass)。
			// 轮廓线负责清理边缘区域，内部填充线保持 width 的间距即可。
			// 加上 EPSILON 防止浮点数刚好落在边界上导致的判断误差
			double currentY = minY + width / 2.0 + EPSILON;

			while (currentY < maxY) {
			List<Double> xIntersections = new ArrayList<>();
			int n = rotatedPoly.size();


			for (int i = 0; i < n; i++) {
			Point p1 = rotatedPoly.get(i);
			Point p2 = rotatedPoly.get((i + 1) % n);

			// 优化：忽略水平线段 (p1.y == p2.y)，避免除零错误，水平线段不参与垂直扫描线求交
			// 忽略水平线段
			if (Math.abs(p1.y - p2.y) < EPSILON) continue;

			// 判断线段是否跨越 currentY
			// 使用严格的不等式逻辑配合范围判断
			double minP = Math.min(p1.y, p2.y);
			double maxP = Math.max(p1.y, p2.y);

			if (currentY >= minP && currentY < maxP) {
			// 线性插值求X
			double x = p1.x + (currentY - p1.y) * (p2.x - p1.x) / (p2.y - p1.y);
			xIntersections.add(x);
			}
			@@ -191,20 +229,19 @@

			Collections.sort(xIntersections);

			// 提取线段，通常是成对出现
			if (xIntersections.size() >= 2) {
			// 取最左和最右点连接（应对可能出现的微小计算误差导致的多个交点）
			// 取最左和最右交点
			double xStart = xIntersections.get(0);
			double xEnd = xIntersections.get(xIntersections.size() - 1);

			// 只有当线段长度大于极小值时才添加，避免生成噪点路径
			if (xEnd - xStart > EPSILON) {
			Point rStart = rotatePoint(new Point(xStart, currentY), angle); // 这里的currentY实际上带了epsilon，还原时没问题
			// 反向旋转回原坐标系
			Point rStart = rotatePoint(new Point(xStart, currentY), angle);
			Point rEnd = rotatePoint(new Point(xEnd, currentY), angle);
			segments.add(new PathSegment(rStart, rEnd, true));
			}
			}

			// 步进
			currentY += width;
			}

			@@ -231,7 +268,6 @@
			// 添加过渡段
			if (i > 0) {
			Point prevEnd = result.get(result.size() - 1).end;
			// 只有当距离确实存在时才添加过渡段（避免重合点）
			if (distanceSq(prevEnd, actualStart) > EPSILON) {
			result.add(new PathSegment(prevEnd, actualStart, false));
			}
			@@ -242,12 +278,12 @@
			return result;
			}

			// ================= 基础几何工具 (优化版) =================
			// ================= 基础几何工具 =================

			private static List<Point> parseCoords(String s) {
			List<Point> list = new ArrayList<>();
			if (s == null \|\| s.trim().isEmpty()) return list;


			String[] parts = s.split(";");
			for (String part : parts) {
			String[] xy = part.split(",");
			@@ -257,14 +293,13 @@
			double y = Double.parseDouble(xy[1].trim());
			list.add(new Point(x, y));
			} catch (NumberFormatException e) {
			// 忽略格式错误的单个点
			// 忽略格式错误
			}
			}
			}
			return list;
			}

			// Shoelace公式判断方向并调整
			private static void ensureCCW(List<Point> polygon) {
			double sum = 0;
			for (int i = 0; i < polygon.size(); i++) {
			@@ -272,14 +307,33 @@
			Point p2 = polygon.get((i + 1) % polygon.size());
			sum += (p2.x - p1.x) * (p2.y + p1.y);
			}
			// 假设标准笛卡尔坐标系，sum > 0 为顺时针，需要反转为逆时针
			if (sum > 0) {
			Collections.reverse(polygon);
			}
			}

			private static List<Point> shrinkPolygon(List<Point> polygon, double margin) {
			List<Point> newPoints = new ArrayList<>();
			int n = polygon.size();

			for (int i = 0; i < n; i++) {
			Point p1 = polygon.get(i);
			Point p2 = polygon.get((i + 1) % n);
			Point p0 = polygon.get((i - 1 + n) % n);

			Line line1 = offsetLine(p1, p2, margin);
			Line line0 = offsetLine(p0, p1, margin);

			Point intersection = getIntersection(line0, line1);
			if (intersection != null) {
			newPoints.add(intersection);
			}
			}
			return newPoints;
			}

			private static class Line {
			double a, b, c;
			double a, b, c;
			public Line(double a, double b, double c) { this.a = a; this.b = b; this.c = c; }
			}

			@@ -287,13 +341,13 @@
			double dx = p2.x - p1.x;
			double dy = p2.y - p1.y;
			double len = Math.sqrt(dx * dx + dy * dy);

			// 防止除以0

			if (len < EPSILON) return new Line(0, 0, 0);

			double nx = -dy / len;
			double ny = dx / len;

			// 向左侧平移（假设逆时针）
			double newX = p1.x + nx * dist;
			double newY = p1.y + ny * dist;

			@@ -305,7 +359,7 @@

			private static Point getIntersection(Line l1, Line l2) {
			double det = l1.a * l2.b - l2.a * l1.b;
			if (Math.abs(det) < EPSILON) return null; // 平行
			if (Math.abs(det) < EPSILON) return null;
			double x = (l1.b * l2.c - l2.b * l1.c) / det;
			double y = (l2.a * l1.c - l1.a * l2.c) / det;
			return new Point(x, y);
			@@ -324,7 +378,7 @@
			}
			return res;
			}


			private static double distanceSq(Point p1, Point p2) {
			return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y);
			}