From f94b1436d7a28c8e28d010b2cb657ab7c064e353 Mon Sep 17 00:00:00 2001
From: 826220679@qq.com <826220679@qq.com>
Date: 星期日, 28 十二月 2025 20:36:38 +0800
Subject: [PATCH] 修改了导航预览
---
src/lujing/YixinglujingHaveObstacel.java | 406 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 401 insertions(+), 5 deletions(-)
diff --git a/src/lujing/YixinglujingHaveObstacel.java b/src/lujing/YixinglujingHaveObstacel.java
index 7d3cbce..5784ef3 100644
--- a/src/lujing/YixinglujingHaveObstacel.java
+++ b/src/lujing/YixinglujingHaveObstacel.java
@@ -54,6 +54,17 @@
finalPath.addAll(scanPath);
+ // 10. 鏍煎紡鍖栧潗鏍囷細淇濈暀涓や綅灏忔暟
+ for (PathSegment segment : finalPath) {
+ segment.start.x = Math.round(segment.start.x * 100.0) / 100.0;
+ segment.start.y = Math.round(segment.start.y * 100.0) / 100.0;
+ segment.end.x = Math.round(segment.end.x * 100.0) / 100.0;
+ segment.end.y = Math.round(segment.end.y * 100.0) / 100.0;
+ }
+
+ // 11. 鎵撳嵃杈撳嚭璺緞鍧愭爣
+ printPathCoordinates(finalPath);
+
return finalPath;
}
@@ -91,8 +102,8 @@
remainingSegments.addAll(clippedSegments);
}
- // 3. 閲嶆柊杩炴帴璺緞娈碉紙寮撳瓧褰㈣繛鎺ワ級
- return reconnectSegments(remainingSegments);
+ // 3. 閲嶆柊杩炴帴璺緞娈碉紙寮撳瓧褰㈣繛鎺ワ紝鏅鸿兘澶勭悊杈圭晫绌胯秺锛�
+ return reconnectSegments(remainingSegments, polygon);
}
/**
@@ -143,7 +154,6 @@
return result;
} else {
// 涓�绔湪鍐呴儴锛屼竴绔湪澶栭儴
- Point insidePoint = startInside ? segment.start : segment.end;
Point outsidePoint = startInside ? segment.end : segment.start;
List<Point> intersections = obstacle.getIntersections(segment);
@@ -167,8 +177,9 @@
/**
* 閲嶆柊杩炴帴璺緞娈碉紝褰㈡垚杩炵画寮撳瓧褰㈣矾寰�
+ * 浼樺寲锛氭櫤鑳藉鐞嗚竟鐣岀┛瓒婏紝褰撴崲琛岃矾寰勭┛瓒婅竟鐣屾椂锛屾部杈圭晫琛岃蛋
*/
- private static List<PathSegment> reconnectSegments(List<PathSegment> segments) {
+ private static List<PathSegment> reconnectSegments(List<PathSegment> segments, List<Point> boundary) {
if (segments.isEmpty()) return new ArrayList<>();
List<PathSegment> reconnected = new ArrayList<>();
@@ -178,7 +189,9 @@
if (seg.isMowing) {
// 鍓茶崏娈碉細妫�鏌ユ槸鍚﹂渶瑕佹坊鍔犵┖璧版
if (distance(currentPos, seg.start) > 0.01) {
- reconnected.add(new PathSegment(currentPos, seg.start, false));
+ // 浣跨敤鏅鸿兘杩炴帴鏂规硶鐢熸垚鎹㈣璺緞
+ List<PathSegment> connectionPath = buildSmartConnection(currentPos, seg.start, boundary);
+ reconnected.addAll(connectionPath);
}
reconnected.add(seg);
currentPos = seg.end;
@@ -193,6 +206,351 @@
}
/**
+ * 鏅鸿兘杩炴帴涓ょ偣锛氬鏋滅洿绾夸笉绌胯秺杈圭晫鍒欑洿鎺ヨ繛鎺ワ紝鍚﹀垯浣跨敤鐩寸嚎+杈圭晫娣峰悎璺緞
+ * 浼樺寲閫昏緫锛�
+ * 1. 濡傛灉AB绾夸笉绌胯秺杈圭晫C锛岀洿鎺ヤ娇鐢ˋB浣滀负鎹㈣璺嚎
+ * 2. 濡傛灉AB绾跨┛瓒婁簡杈圭晫C锛屾壘鍒版墍鏈変氦鐐癸紝灏咥B鍒嗘垚澶氫釜娈�
+ * - 瀵逛簬鍦ㄨ竟鐣屽唴閮ㄧ殑娈碉紙濡侱F娈点�丟H娈碉級锛屾部杈圭晫琛岃蛋
+ * - 瀵逛簬鍦ㄨ竟鐣屽閮ㄧ殑娈碉紝娌緼B鐩寸嚎琛岃蛋
+ * 璺緞绀轰緥锛欰 鈫� D(鐩寸嚎) 鈫� F(娌胯竟鐣�) 鈫� G(鐩寸嚎) 鈫� H(娌胯竟鐣�) 鈫� B(鐩寸嚎)
+ *
+ * @param pointA 璧风偣锛堜笂涓�娈电粨鏉熺殑缁堢偣锛�
+ * @param pointB 缁堢偣锛堜笅涓�娈甸渶瑕佸壊鑽夎矾寰勭殑璧峰鐐癸級
+ * @param boundary 瀹夊叏鍐呯缉杈圭晫C
+ * @return 杩炴帴璺緞娈靛垪琛紙鍏ㄩ儴涓篿sMowing=false鐨勭┖璧版锛�
+ */
+ private static List<PathSegment> buildSmartConnection(Point pointA, Point pointB, List<Point> boundary) {
+ List<PathSegment> result = new ArrayList<>();
+
+ // 1. 妫�鏌B鐩寸嚎鏄惁绌胯秺杈圭晫C
+ if (!segmentIntersectsBoundary(pointA, pointB, boundary)) {
+ // 涓嶇┛瓒婅竟鐣岋紝鐩存帴浣跨敤AB浣滀负鎹㈣璺嚎
+ result.add(new PathSegment(pointA, pointB, false));
+ return result;
+ }
+
+ // 2. AB绾跨┛瓒婁簡杈圭晫C锛岄渶瑕佹壘鍒版墍鏈変氦鐐�
+ List<IntersectionInfo> intersections = getAllBoundaryIntersections(pointA, pointB, boundary);
+
+ if (intersections.isEmpty()) {
+ // 娌℃湁浜ょ偣锛堜笉搴旇鍙戠敓锛屼絾瀹夊叏澶勭悊锛夛紝浣跨敤鐩寸嚎
+ result.add(new PathSegment(pointA, pointB, false));
+ return result;
+ }
+
+ // 3. 鎸夎窛绂昏捣鐐笰鐨勮窛绂绘帓搴忎氦鐐�
+ intersections.sort(Comparator.comparingDouble(inter -> distance(pointA, inter.point)));
+
+ // 4. 鏋勫缓瀹屾暣鐨勭偣搴忓垪锛欰, I1, I2, ..., In, B锛圛涓轰氦鐐癸級
+ List<Point> pointSequence = new ArrayList<>();
+ pointSequence.add(pointA);
+ for (IntersectionInfo inter : intersections) {
+ pointSequence.add(inter.point);
+ }
+ pointSequence.add(pointB);
+
+ // 5. 澶勭悊姣忎袱涓浉閭荤偣涔嬮棿鐨勬
+ Point currentPos = pointA;
+
+ for (int i = 0; i < pointSequence.size() - 1; i++) {
+ Point p1 = pointSequence.get(i);
+ Point p2 = pointSequence.get(i + 1);
+
+ // 鍒ゆ柇p1鍒皃2鐨勬锛圓B绾挎鐨勪竴閮ㄥ垎锛夋槸鍚﹀湪杈圭晫C鍐呴儴锛堟鏌ヤ腑鐐癸級
+ Point midPoint = new Point((p1.x + p2.x) / 2, (p1.y + p2.y) / 2);
+ boolean segmentInsideBoundary = isPointInPolygon(midPoint, boundary);
+
+ if (segmentInsideBoundary) {
+ // 娈靛湪杈圭晫鍐呴儴锛堝DF娈点�丟H娈碉級锛岄渶瑕佹部杈圭晫琛岃蛋
+ if (i == 0) {
+ // 绗竴涓锛氫粠A鍒扮涓�涓氦鐐笵
+ // 濡傛灉娈靛湪杈圭晫鍐呴儴锛岃鏄嶢鍦ㄨ竟鐣屽唴閮紝闇�瑕佸厛浠嶢娌胯竟鐣岃蛋鍒扮涓�涓氦鐐�
+ IntersectionInfo firstInter = intersections.get(0);
+ SnapResult snapA = snapToBoundary(currentPos, boundary);
+ List<PathSegment> boundaryPath = getBoundaryPathBetweenPoints(
+ snapA.onEdge, snapA.edgeIndex,
+ firstInter.point, firstInter.edgeIndex,
+ boundary);
+ result.addAll(boundaryPath);
+ currentPos = firstInter.point;
+ } else if (i == pointSequence.size() - 2) {
+ // 鏈�鍚庝竴涓锛氫粠鏈�鍚庝竴涓氦鐐笻鍒癇
+ // 闇�瑕佹部杈圭晫浠庡綋鍓嶇偣锛堝簲璇ユ槸鏈�鍚庝竴涓氦鐐笻锛夊埌B鍦ㄨ竟鐣屼笂鐨勬姇褰�
+ IntersectionInfo lastInter = intersections.get(intersections.size() - 1);
+ SnapResult snapB = snapToBoundary(pointB, boundary);
+ List<PathSegment> boundaryPath = getBoundaryPathBetweenPoints(
+ currentPos, lastInter.edgeIndex,
+ snapB.onEdge, snapB.edgeIndex,
+ boundary);
+ result.addAll(boundaryPath);
+ // 濡傛灉B涓嶅湪杈圭晫涓婏紝浠庤竟鐣屾姇褰辩洿绾垮埌B
+ if (distance(snapB.onEdge, pointB) > 1e-6) {
+ result.add(new PathSegment(snapB.onEdge, pointB, false));
+ }
+ currentPos = pointB;
+ } else {
+ // 涓棿娈碉細涓や釜浜ょ偣涔嬮棿鐨勬锛堥兘鍦ㄨ竟鐣屼笂锛夛紝娌胯竟鐣岃璧�
+ IntersectionInfo inter1 = intersections.get(i - 1);
+ IntersectionInfo inter2 = intersections.get(i);
+ List<PathSegment> boundaryPath = getBoundaryPathBetweenPoints(
+ inter1.point, inter1.edgeIndex,
+ inter2.point, inter2.edgeIndex,
+ boundary);
+ result.addAll(boundaryPath);
+ currentPos = inter2.point;
+ }
+ } else {
+ // 娈靛湪杈圭晫澶栭儴锛屽彲浠ョ洿鎺ユ部AB鐩寸嚎杩炴帴锛堝A鍒癉锛孎鍒癎锛孒鍒癇锛�
+ if (distance(p1, p2) > 1e-6) {
+ // 濡傛灉褰撳墠鐐逛笉鍦╬1锛屽厛杩炴帴鍒皃1
+ if (distance(currentPos, p1) > 1e-6) {
+ result.add(new PathSegment(currentPos, p1, false));
+ }
+ // 浠巔1鐩寸嚎鍒皃2
+ result.add(new PathSegment(p1, p2, false));
+ currentPos = p2;
+ }
+ }
+ }
+
+ return result;
+ }
+
+ /**
+ * 妫�鏌ョ嚎娈垫槸鍚︾┛瓒婅竟鐣岋紙涓庤竟鐣岃竟鐩镐氦锛屼笉鍖呮嫭绔偣锛�
+ */
+ private static boolean segmentIntersectsBoundary(Point a, Point b, List<Point> boundary) {
+ for (int i = 0; i < boundary.size(); i++) {
+ Point c = boundary.get(i);
+ Point d = boundary.get((i + 1) % boundary.size());
+ // 蹇界暐鍏变韩绔偣鐨勭浉浜�
+ if (isSamePoint(a, c) || isSamePoint(a, d) || isSamePoint(b, c) || isSamePoint(b, d)) {
+ continue;
+ }
+ if (segmentsIntersect(a, b, c, d)) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * 鑾峰彇绾挎涓庤竟鐣岀殑鎵�鏈変氦鐐逛俊鎭紙鍖呮嫭鐐瑰拰瀵瑰簲杈圭储寮曪級
+ */
+ private static List<IntersectionInfo> getAllBoundaryIntersections(Point a, Point b, List<Point> boundary) {
+ List<IntersectionInfo> intersections = new ArrayList<>();
+
+ for (int i = 0; i < boundary.size(); i++) {
+ Point c = boundary.get(i);
+ Point d = boundary.get((i + 1) % boundary.size());
+
+ // 蹇界暐鍏变韩绔偣
+ if (isSamePoint(a, c) || isSamePoint(a, d) || isSamePoint(b, c) || isSamePoint(b, d)) {
+ continue;
+ }
+
+ Point intersection = getLineIntersection(a, b, c, d);
+ if (intersection != null) {
+ intersections.add(new IntersectionInfo(intersection, i));
+ }
+ }
+
+ return intersections;
+ }
+
+ /**
+ * 鑾峰彇杈圭晫涓婁袱鐐逛箣闂寸殑璺緞锛堟部杈圭晫琛岃蛋锛�
+ * @param start 璧风偣锛堝繀椤诲湪杈圭晫涓婏級
+ * @param startEdgeIndex 璧风偣鎵�鍦ㄧ殑杈圭储寮�
+ * @param end 缁堢偣锛堝繀椤诲湪杈圭晫涓婏級
+ * @param endEdgeIndex 缁堢偣鎵�鍦ㄧ殑杈圭储寮�
+ * @param boundary 杈圭晫鐐瑰垪琛�
+ * @return 娌胯竟鐣岀殑璺緞娈靛垪琛�
+ */
+ private static List<PathSegment> getBoundaryPathBetweenPoints(
+ Point start, int startEdgeIndex,
+ Point end, int endEdgeIndex,
+ List<Point> boundary) {
+
+ List<PathSegment> result = new ArrayList<>();
+
+ if (startEdgeIndex == endEdgeIndex) {
+ // 鍦ㄥ悓涓�鏉¤竟涓婏紝鐩存帴杩炴帴
+ if (distance(start, end) > 1e-6) {
+ result.add(new PathSegment(start, end, false));
+ }
+ return result;
+ }
+
+ int n = boundary.size();
+
+ // 璁＄畻椤烘椂閽堣矾寰�
+ List<Point> pathClockwise = new ArrayList<>();
+ pathClockwise.add(start);
+
+ int curr = startEdgeIndex;
+ while (curr != endEdgeIndex) {
+ pathClockwise.add(boundary.get((curr + 1) % n));
+ curr = (curr + 1) % n;
+ }
+ pathClockwise.add(end);
+
+ // 璁＄畻閫嗘椂閽堣矾寰�
+ List<Point> pathCounterClockwise = new ArrayList<>();
+ pathCounterClockwise.add(start);
+ curr = startEdgeIndex;
+ while (curr != endEdgeIndex) {
+ pathCounterClockwise.add(boundary.get(curr));
+ curr = (curr - 1 + n) % n;
+ }
+ pathCounterClockwise.add(end);
+
+ // 閫夋嫨杈冪煭鐨勮矾寰�
+ List<Point> chosenPath = getPathLength(pathClockwise) < getPathLength(pathCounterClockwise)
+ ? pathClockwise : pathCounterClockwise;
+
+ // 杞崲涓鸿矾寰勬
+ for (int i = 0; i < chosenPath.size() - 1; i++) {
+ if (distance(chosenPath.get(i), chosenPath.get(i + 1)) > 1e-6) {
+ result.add(new PathSegment(chosenPath.get(i), chosenPath.get(i + 1), false));
+ }
+ }
+
+ return result;
+ }
+
+ /**
+ * 璁＄畻璺緞鎬婚暱搴�
+ */
+ private static double getPathLength(List<Point> path) {
+ double len = 0;
+ for (int i = 0; i < path.size() - 1; i++) {
+ len += distance(path.get(i), path.get(i + 1));
+ }
+ return len;
+ }
+
+ /**
+ * 鍒ゆ柇涓や釜鐐规槸鍚︾浉鍚岋紙鑰冭檻娴偣璇樊锛�
+ */
+ private static boolean isSamePoint(Point a, Point b) {
+ return Math.abs(a.x - b.x) < 1e-6 && Math.abs(a.y - b.y) < 1e-6;
+ }
+
+ /**
+ * 鍒ゆ柇涓ゆ潯绾挎鏄惁鐩镐氦锛堜笉鍖呮嫭绔偣锛�
+ */
+ private static boolean segmentsIntersect(Point a, Point b, Point c, Point d) {
+ return ccw(a, c, d) != ccw(b, c, d) && ccw(a, b, c) != ccw(a, b, d);
+ }
+
+ /**
+ * 鍒ゆ柇涓夌偣鏄惁閫嗘椂閽堟帓鍒�
+ */
+ private static boolean ccw(Point a, Point b, Point c) {
+ return (c.y - a.y) * (b.x - a.x) > (b.y - a.y) * (c.x - a.x);
+ }
+
+ /**
+ * 浜ょ偣淇℃伅鍐呴儴绫�
+ */
+ private static class IntersectionInfo {
+ Point point; // 浜ょ偣鍧愭爣
+ int edgeIndex; // 浜ょ偣鎵�鍦ㄧ殑杈圭晫杈圭储寮�
+
+ IntersectionInfo(Point point, int edgeIndex) {
+ this.point = point;
+ this.edgeIndex = edgeIndex;
+ }
+ }
+
+ /**
+ * 杈圭晫鍚搁檮缁撴灉鍐呴儴绫�
+ */
+ private static class SnapResult {
+ Point onEdge; // 鍦ㄨ竟鐣屼笂鐨勬姇褰辩偣
+ int edgeIndex; // 鎵�鍦ㄧ殑杈圭晫杈圭储寮�
+
+ SnapResult(Point p, int idx) {
+ this.onEdge = p;
+ this.edgeIndex = idx;
+ }
+ }
+
+ /**
+ * 璁＄畻鐐瑰埌杈圭晫鏈�杩戠殑鎶曞奖鐐逛互鍙婃墍鍦ㄨ竟绱㈠紩
+ * @param p 瑕佸惛闄勭殑鐐�
+ * @param poly 杈圭晫澶氳竟褰�
+ * @return 鍚搁檮缁撴灉
+ */
+ private static SnapResult snapToBoundary(Point p, List<Point> poly) {
+ double minD = Double.MAX_VALUE;
+ Point bestProj = p;
+ int bestIdx = -1;
+ for (int i = 0; i < poly.size(); i++) {
+ Point s = poly.get(i);
+ Point e = poly.get((i + 1) % poly.size());
+ double l2 = (s.x - e.x) * (s.x - e.x) + (s.y - e.y) * (s.y - e.y);
+ if (l2 < 1e-10) {
+ double d = Math.hypot(p.x - s.x, p.y - s.y);
+ if (d < minD) {
+ minD = d;
+ bestProj = s;
+ bestIdx = i;
+ }
+ continue;
+ }
+ double t = ((p.x - s.x) * (e.x - s.x) + (p.y - s.y) * (e.y - s.y)) / l2;
+ t = Math.max(0, Math.min(1, t));
+ Point proj = new Point(s.x + t * (e.x - s.x), s.y + t * (e.y - s.y));
+ double d = Math.hypot(p.x - proj.x, p.y - proj.y);
+ if (d < minD) {
+ minD = d;
+ bestProj = proj;
+ bestIdx = i;
+ }
+ }
+ return new SnapResult(bestProj, bestIdx == -1 ? 0 : bestIdx);
+ }
+
+ /**
+ * 鍒ゆ柇鐐规槸鍚﹀湪杈圭晫涓婏紙璺濈杈圭晫寰堣繎锛�
+ * @param p 瑕佹鏌ョ殑鐐�
+ * @param boundary 杈圭晫澶氳竟褰�
+ * @return 鏄惁鍦ㄨ竟鐣屼笂
+ */
+ @SuppressWarnings("unused")
+ private static boolean isPointOnBoundary(Point p, List<Point> boundary) {
+ double threshold = 1e-4; // 闃堝�硷紝鑰冭檻娴偣璇樊
+ for (int i = 0; i < boundary.size(); i++) {
+ Point s = boundary.get(i);
+ Point e = boundary.get((i + 1) % boundary.size());
+ double dist = distToSegment(p, s, e);
+ if (dist < threshold) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * 璁＄畻鐐瑰埌绾挎鐨勮窛绂�
+ * @param p 鐐�
+ * @param s 绾挎璧风偣
+ * @param e 绾挎缁堢偣
+ * @return 璺濈
+ */
+ private static double distToSegment(Point p, Point s, Point e) {
+ double l2 = (s.x - e.x) * (s.x - e.x) + (s.y - e.y) * (s.y - e.y);
+ if (l2 < 1e-10) {
+ return Math.hypot(p.x - s.x, p.y - s.y);
+ }
+ double t = ((p.x - s.x) * (e.x - s.x) + (p.y - s.y) * (e.y - s.y)) / l2;
+ t = Math.max(0, Math.min(1, t));
+ return Math.hypot(p.x - (s.x + t * (e.x - s.x)), p.y - (s.y + t * (e.y - s.y)));
+ }
+
+ /**
* 鐢熸垚鍘熷鎵弿璺緞锛堟棤闅滅鐗╃増鏈級
*/
private static List<PathSegment> generateGlobalScanPath(
@@ -610,6 +968,44 @@
return points;
}
+ /**
+ * 鎵撳嵃杈撳嚭璺緞鍧愭爣鍒版帶鍒跺彴
+ */
+ private static void printPathCoordinates(List<PathSegment> path) {
+ if (path == null || path.isEmpty()) {
+ System.out.println("璺緞涓虹┖");
+ return;
+ }
+
+ System.out.println("========== 璺緞鍧愭爣杈撳嚭 ==========");
+ System.out.println("鎬昏矾寰勬鏁�: " + path.size());
+ System.out.println();
+ System.out.println("璺緞鍧愭爣搴忓垪 (鏍煎紡: x,y;x,y;...):");
+
+ StringBuilder sb = new StringBuilder();
+ for (int i = 0; i < path.size(); i++) {
+ PathSegment segment = path.get(i);
+ if (i == 0) {
+ // 绗竴涓鐨勮捣鐐�
+ sb.append(String.format("%.2f,%.2f", segment.start.x, segment.start.y));
+ }
+ // 姣忎釜娈电殑缁堢偣
+ sb.append(";");
+ sb.append(String.format("%.2f,%.2f", segment.end.x, segment.end.y));
+ }
+
+ System.out.println(sb.toString());
+ System.out.println();
+ System.out.println("璇︾粏璺緞淇℃伅:");
+ for (int i = 0; i < path.size(); i++) {
+ PathSegment segment = path.get(i);
+ String type = segment.isMowing ? "鍓茶崏" : "绌鸿蛋";
+ System.out.println(String.format("娈� %d [%s]: (%.2f,%.2f) -> (%.2f,%.2f)",
+ i + 1, type, segment.start.x, segment.start.y, segment.end.x, segment.end.y));
+ }
+ System.out.println("==================================");
+ }
+
public static class Point {
public double x, y;
public Point(double x, double y) { this.x = x; this.y = y; }
--
Gitblit v1.10.0