| | |
| | | # 割草机地块障碍物配置文件 |
| | | # 生成时间:2025-12-25T16:03:28.188601 |
| | | # 生成时间:2025-12-25T16:37:39.564155300 |
| | | # 坐标系:WGS84(度分格式) |
| | | |
| | | # ============ 地块基准站配置 ============ |
| | |
| | | # 格式:plot.[地块编号].obstacle.[障碍物名称].originalCoords=[坐标串] |
| | | # 格式:plot.[地块编号].obstacle.[障碍物名称].xyCoords=[坐标串] |
| | | |
| | | # --- 地块LAND1的障碍物 --- |
| | | plot.LAND1.obstacle.障碍物1.shape=1 |
| | | plot.LAND1.obstacle.障碍物1.originalCoords=0.000000,N;0.000000,E;0.000000,N;0.000000,E;0.000000,N;0.000000,E;0.000000,N;0.000000,E;0.000000,N;0.000000,E |
| | | plot.LAND1.obstacle.障碍物1.xyCoords=143.09,-434.11;142.20,-387.55;186.08,-379.49;224.58,-418.89;201.30,-447.54 |
| | | |
| | |
| | | #Dikuai Properties |
| | | #Thu Dec 25 16:03:28 CST 2025 |
| | | #Thu Dec 25 16:37:39 CST 2025 |
| | | LAND1.angleThreshold=-1 |
| | | LAND1.baseStationCoordinates=3949.89151752,N,11616.79267501,E |
| | | LAND1.boundaryCoordinates=-58.32,-476.37;85.99,-289.22;354.31,-329.80;293.43,-559.79 |
| | |
| | | LAND1.mowingPattern=平行线 |
| | | LAND1.mowingSafetyDistance=0.53 |
| | | LAND1.mowingTrack=-1 |
| | | LAND1.mowingWidth=200 |
| | | LAND1.plannedPath=292.652,-558.038;-56.017,-475.349;-54.677,-473.611;293.164,-556.104;293.676,-554.170;-53.337,-471.874;-51.998,-470.136;294.188,-552.236;294.700,-550.302;-50.658,-468.398;-49.318,-466.660;295.212,-548.368;295.724,-546.434;-47.978,-464.923;-46.638,-463.185;296.236,-544.500;296.748,-542.566;-45.298,-461.447;-43.958,-459.710;297.260,-540.632;297.772,-538.698;-42.618,-457.972;-41.278,-456.234;298.284,-536.764;298.796,-534.830;-39.938,-454.497;-38.598,-452.759;299.308,-532.896;299.820,-530.962;-37.258,-451.021;-35.918,-449.283;300.332,-529.028;300.844,-527.093;-34.578,-447.546;-33.239,-445.808;301.356,-525.159;301.868,-523.225;-31.899,-444.070;-30.559,-442.333;302.380,-521.291;302.892,-519.357;-29.219,-440.595;-27.879,-438.857;303.403,-517.423;303.915,-515.489;-26.539,-437.120;-25.199,-435.382;304.427,-513.555;304.939,-511.621;-23.859,-433.644;-22.519,-431.906;305.451,-509.687;305.963,-507.753;-21.179,-430.169;-19.839,-428.431;306.475,-505.819;306.987,-503.885;-18.499,-426.693;-17.159,-424.956;307.499,-501.951;308.011,-500.017;-15.819,-423.218;-14.480,-421.480;308.523,-498.083;309.035,-496.148;-13.140,-419.742;-11.800,-418.005;309.547,-494.214;310.059,-492.280;-10.460,-416.267;-9.120,-414.529;310.571,-490.346;311.083,-488.412;-7.780,-412.792;-6.440,-411.054;311.595,-486.478;312.107,-484.544;-5.100,-409.316;-3.760,-407.579;312.619,-482.610;313.131,-480.676;-2.420,-405.841;-1.080,-404.103;313.643,-478.742;314.155,-476.808;0.260,-402.365;1.600,-400.628;142.559,-434.057;142.519,-431.992;2.940,-398.890;4.279,-397.152;142.480,-429.928;142.440,-427.863;5.619,-395.415;6.959,-393.677;142.401,-425.798;142.362,-423.733;8.299,-391.939;9.639,-390.202;142.322,-421.668;142.283,-419.603;10.979,-388.464;12.319,-386.726;142.243,-417.539;142.204,-415.474;13.659,-384.988;14.999,-383.251;142.164,-413.409;142.125,-411.344;16.339,-381.513;17.679,-379.775;142.085,-409.279;142.046,-407.214;19.019,-378.038;20.359,-376.300;142.006,-405.150;141.967,-403.085;21.699,-374.562;23.038,-372.825;141.927,-401.020;141.888,-398.955;24.378,-371.087;25.718,-369.349;141.848,-396.890;141.809,-394.825;27.058,-367.611;28.398,-365.874;141.770,-392.761;141.730,-390.696;29.738,-364.136;31.078,-362.398;141.691,-388.631;141.679,-387.454;141.708,-387.352;141.758,-387.258;141.825,-387.175;141.908,-387.108;142.002,-387.058;142.948,-386.874;32.418,-360.661;33.758,-358.923;147.832,-385.977;152.717,-385.079;35.098,-357.185;36.438,-355.448;157.601,-384.182;162.485,-383.285;37.778,-353.710;39.118,-351.972;167.369,-382.388;172.254,-381.491;40.458,-350.234;41.797,-348.497;177.138,-380.594;182.022,-379.697;43.137,-346.759;44.477,-345.021;331.049,-412.984;331.561,-411.050;45.817,-343.284;47.157,-341.546;332.073,-409.116;332.585,-407.182;48.497,-339.808;49.837,-338.071;333.097,-405.248;333.609,-403.314;51.177,-336.333;52.517,-334.595;334.121,-401.380;334.633,-399.446;53.857,-332.857;55.197,-331.120;335.145,-397.511;335.657,-395.577;56.537,-329.382;57.877,-327.644;336.169,-393.643;336.681,-391.709;59.217,-325.907;60.556,-324.169;337.193,-389.775;337.705,-387.841;61.896,-322.431;63.236,-320.694;338.217,-385.907;338.729,-383.973;64.576,-318.956;65.916,-317.218;339.241,-382.039;339.753,-380.105;67.256,-315.480;68.596,-313.743;340.265,-378.171;340.777,-376.237;69.936,-312.005;71.276,-310.267;341.288,-374.303;341.800,-372.369;72.616,-308.530;73.956,-306.792;342.312,-370.435;342.824,-368.501;75.296,-305.054;76.636,-303.317;343.336,-366.567;343.848,-364.632;77.976,-301.579;79.315,-299.841;344.360,-362.698;344.872,-360.764;80.655,-298.103;81.995,-296.366;345.384,-358.830;345.896,-356.896;83.335,-294.628;84.675,-292.890;346.408,-354.962;346.920,-353.028;86.015,-291.153;101.614,-292.797;347.432,-351.094;347.944,-349.160;125.537,-296.415;149.460,-300.033;348.456,-347.226;348.968,-345.292;173.384,-303.651;197.307,-307.269;349.480,-343.358;349.992,-341.424;221.230,-310.887;245.153,-314.505;350.504,-339.490;351.016,-337.556;269.076,-318.123;293.000,-321.741;351.528,-335.622;352.040,-333.688;316.923,-325.359;340.846,-328.977;352.552,-331.753;222.518,-416.022;323.882,-440.061;324.394,-438.127;219.904,-413.346;217.289,-410.671;324.906,-436.193;325.418,-434.259;214.675,-407.995;212.061,-405.320;325.930,-432.325;326.442,-430.390;209.446,-402.644;206.832,-399.969;326.954,-428.456;327.466,-426.522;204.218,-397.293;201.603,-394.618;327.978,-424.588;328.489,-422.654;198.989,-391.942;196.374,-389.267;329.001,-420.720;329.513,-418.786;193.760,-386.591;191.146,-383.916;330.025,-416.852;330.537,-414.918;188.531,-381.240;224.959,-418.520;225.027,-418.606;225.076,-418.704;225.104,-418.811;225.109,-418.921;225.091,-419.029;225.051,-419.132;224.991,-419.224;201.642,-447.944;201.490,-448.033;314.667,-474.874;315.179,-472.940;202.968,-446.328;204.368,-444.605;315.691,-471.006;316.202,-469.072;205.768,-442.881;207.169,-441.158;316.714,-467.138;317.226,-465.204;208.569,-439.435;209.969,-437.711;317.738,-463.269;318.250,-461.335;211.370,-435.988;212.770,-434.265;318.762,-459.401;319.274,-457.467;214.170,-432.541;215.571,-430.818;319.786,-455.533;320.298,-453.599;216.971,-429.094;218.371,-427.371;320.810,-451.665;321.322,-449.731;219.772,-425.648;221.172,-423.924;321.834,-447.797;322.346,-445.863;222.573,-422.201;223.973,-420.478;322.858,-443.929;323.370,-441.995;225.065,-418.681 |
| | | LAND1.mowingWidth=500 |
| | | LAND1.obstacleCoordinates=-1 |
| | | LAND1.plannedPath=353.646421,-330.235669;86.219211,-289.790692;-57.399120,-476.043693;293.049800,-559.155132;353.646421,-330.235669;352.359360,-335.097876;82.727142,-294.319420;79.235073,-298.848147;351.072299,-339.960083;349.785238,-344.822290;75.743005,-303.376874;72.250936,-307.905602;348.498177,-349.684497;347.211116,-354.546703;68.758867,-312.434329;65.266799,-316.963057;345.924055,-359.408910;344.636994,-364.271117;61.774730,-321.491784;58.282661,-326.020511;343.349933,-369.133324;342.062872,-373.995531;54.790593,-330.549239;51.298524,-335.077966;340.775811,-378.857738;339.488750,-383.719945;47.806455,-339.606694;44.314387,-344.135421;338.201689,-388.582152;336.914628,-393.444358;40.822318,-348.664148;37.330249,-353.192876;335.627567,-398.306565;334.340506,-403.168772;33.838181,-357.721603;30.346112,-362.250331;333.053445,-408.030979;331.766384,-412.893186;26.854043,-366.779058;23.361975,-371.307785;330.479324,-417.755393;329.192263,-422.617600;19.869906,-375.836513;16.377837,-380.365240;327.905202,-427.479807;326.618141,-432.342013;12.885769,-384.893968;9.393700,-389.422695;325.331080,-437.204220;324.044019,-442.066427;5.901631,-393.951422;2.409563,-398.480150;322.756958,-446.928634;321.469897,-451.790841;-1.082506,-403.008877;-4.574575,-407.537605;320.182836,-456.653048;318.895775,-461.515255;-8.066643,-412.066332;-11.558712,-416.595059;317.608714,-466.377461;316.321653,-471.239668;-15.050781,-421.123787;-18.542849,-425.652514;315.034592,-476.101875;313.747531,-480.964082;-22.034918,-430.181242;-25.526987,-434.709969;312.460470,-485.826289;311.173409,-490.688496;-29.019055,-439.238696;-32.511124,-443.767424;309.886348,-495.550703;308.599287,-500.412910;-36.003193,-448.296151;-39.495261,-452.824879;307.312226,-505.275116;306.025165,-510.137323;-42.987330,-457.353606;-46.479399,-461.882333;304.738104,-514.999530;303.451043,-519.861737;-49.971467,-466.411061;-53.463536,-470.939788;302.163982,-524.723944;300.876921,-529.586151;-56.955605,-475.468516;-6.018549,-488.228958;299.589860,-534.448358;298.302799,-539.310565;52.837057,-502.186981;111.692662,-516.145005;297.015738,-544.172771;295.728677,-549.034978;170.548268,-530.103028;229.403874,-544.061051;294.441616,-553.897185 |
| | | LAND1.returnPathCoordinates=-1 |
| | | LAND1.returnPathRawCoordinates=-1 |
| | | LAND1.returnPointCoordinates=-1 |
| | | LAND1.updateTime=2025-12-25 16\:03\:28 |
| | | LAND1.updateTime=2025-12-25 16\:37\:39 |
| | | LAND1.userId=-1 |
| | |
| | | #Mower Configuration Properties - Updated |
| | | #Thu Dec 25 16:03:36 CST 2025 |
| | | #Thu Dec 25 16:37:55 CST 2025 |
| | | appVersion=-1 |
| | | boundaryLengthVisible=false |
| | | currentWorkLandNumber=LAND1 |
| | |
| | | handheldMarkerId=1872 |
| | | idleTrailDurationSeconds=60 |
| | | manualBoundaryDrawingMode=false |
| | | mapScale=0.05 |
| | | mapScale=11.95 |
| | | measurementModeEnabled=false |
| | | mowerId=6258 |
| | | mowerId=6288 |
| | | serialAutoConnect=true |
| | | serialBaudRate=115200 |
| | | serialPortName=COM15 |
| | | simCardNumber=-1 |
| | | viewCenterX=-5443.82 |
| | | viewCenterY=-3798.20 |
| | | viewCenterX=-148.00 |
| | | viewCenterY=424.51 |
| | |
| | | String gpsRaw = gpsData.getGps_raw(); |
| | | UDPServer.processSerialData(gpsRaw); |
| | | gecaojistatus.parseStatus(gpsData.getStatus()); |
| | | gpsData.getImu_data(); |
| | | } |
| | | |
| | | } |
| | |
| | | private String mowingOverlapDistance; |
| | | // 割草安全距离(米) |
| | | private String mowingSafetyDistance; |
| | | // 障碍物坐标 |
| | | private String obstacleCoordinates; |
| | | |
| | | // 存储多个地块的映射表,键为地块编号 |
| | | private static Map<String, Dikuai> dikuaiMap = new HashMap<>(); |
| | |
| | | dikuai.mowingBladeWidth = landProps.getProperty("mowingBladeWidth", "0.40"); |
| | | dikuai.mowingOverlapDistance = landProps.getProperty("mowingOverlapDistance", "0.06"); |
| | | dikuai.mowingSafetyDistance = landProps.getProperty("mowingSafetyDistance", "-1"); |
| | | dikuai.obstacleCoordinates = landProps.getProperty("obstacleCoordinates", "-1"); |
| | | |
| | | dikuaiMap.put(landNum, dikuai); |
| | | } |
| | |
| | | case "mowingSafetyDistance": |
| | | this.mowingSafetyDistance = value; |
| | | return true; |
| | | case "obstacleCoordinates": |
| | | this.obstacleCoordinates = value; |
| | | return true; |
| | | default: |
| | | System.err.println("未知字段: " + fieldName); |
| | | return false; |
| | |
| | | if (dikuai.mowingBladeWidth != null) properties.setProperty(landNumber + ".mowingBladeWidth", dikuai.mowingBladeWidth); |
| | | if (dikuai.mowingOverlapDistance != null) properties.setProperty(landNumber + ".mowingOverlapDistance", dikuai.mowingOverlapDistance); |
| | | if (dikuai.mowingSafetyDistance != null) properties.setProperty(landNumber + ".mowingSafetyDistance", dikuai.mowingSafetyDistance); |
| | | if (dikuai.obstacleCoordinates != null) properties.setProperty(landNumber + ".obstacleCoordinates", dikuai.obstacleCoordinates); |
| | | } |
| | | |
| | | try { |
| | |
| | | this.mowingSafetyDistance = mowingSafetyDistance; |
| | | } |
| | | |
| | | public String getObstacleCoordinates() { |
| | | return obstacleCoordinates; |
| | | } |
| | | |
| | | public void setObstacleCoordinates(String obstacleCoordinates) { |
| | | this.obstacleCoordinates = obstacleCoordinates; |
| | | } |
| | | |
| | | @Override |
| | | public String toString() { |
| | | return "Dikuai{" + |
| | |
| | | ", updateTime='" + updateTime + '\'' + |
| | | ", mowingPattern='" + mowingPattern + '\'' + |
| | | ", mowingWidth='" + mowingWidth + '\'' + |
| | | ", obstacleCoordinates='" + obstacleCoordinates + '\'' + |
| | | '}'; |
| | | } |
| | | } |
| | |
| | | return false; |
| | | } |
| | | |
| | | // 同时保存到 dikuai.properties |
| | | saveFieldAndRefresh(dikuai, "obstacleCoordinates", obstaclePayload); |
| | | |
| | | obstacleSummaryCache = loadObstacleSummaries(); |
| | | boolean isCurrent = landNumber.equals(currentWorkLandNumber); |
| | | loadDikuaiData(); |
| | |
| | | import java.util.ArrayList; |
| | | import zhuye.Shouye; |
| | | import zhuye.MapRenderer; |
| | | import gecaoji.Device; |
| | | import gecaoji.Gecaoji; |
| | | import gecaoji.lujingdraw; |
| | | import publicway.buttonset; |
| | |
| | | return; |
| | | } |
| | | |
| | | // 解析路径坐标 |
| | | pathPoints = lujingdraw.parsePlannedPath(plannedPath); |
| | | if (pathPoints == null || pathPoints.size() < 2) { |
| | | // 1. 解析路径坐标(原始路径) |
| | | List<Point2D.Double> rawPathPoints = lujingdraw.parsePlannedPath(plannedPath); |
| | | if (rawPathPoints == null || rawPathPoints.size() < 2) { |
| | | JOptionPane.showMessageDialog(null, "路径坐标解析失败,无法进行导航预览", "错误", JOptionPane.ERROR_MESSAGE); |
| | | return; |
| | | } |
| | | |
| | | // 2. 尝试重新生成完整路径段(包含围边和作业路径) |
| | | // 这样可以确保导航预览时,割草机先沿着内缩边界走一圈,再走割草路径 |
| | | List<lujing.Lunjingguihua.PathSegment> segments = null; |
| | | String boundaryCoords = dikuai.getBoundaryCoordinates(); |
| | | String mowingWidth = dikuai.getMowingBladeWidth(); // 注意:这里应该用割草宽度,而不是割刀宽度,通常是一样的 |
| | | // 如果没有割草宽度,尝试从Device获取 |
| | | if (mowingWidth == null || mowingWidth.trim().isEmpty() || "-1".equals(mowingWidth.trim())) { |
| | | Device device = Device.getActiveDevice(); |
| | | if (device != null) { |
| | | mowingWidth = device.getMowingWidth(); |
| | | } |
| | | } |
| | | // 如果还是没有,使用默认值 |
| | | if (mowingWidth == null || mowingWidth.trim().isEmpty() || "-1".equals(mowingWidth.trim())) { |
| | | mowingWidth = "0.34"; |
| | | } |
| | | |
| | | String safetyDistance = dikuai.getMowingSafetyDistance(); |
| | | String obstaclesCoords = dikuai.getObstacleCoordinates(); |
| | | String mowingPattern = dikuai.getMowingPattern(); |
| | | |
| | | if (boundaryCoords != null && !boundaryCoords.trim().isEmpty() && !"-1".equals(boundaryCoords.trim())) { |
| | | try { |
| | | // 解析割草模式 |
| | | String mode = "parallel"; // 默认平行模式 |
| | | if (mowingPattern != null && !mowingPattern.trim().isEmpty()) { |
| | | String pattern = mowingPattern.trim().toLowerCase(); |
| | | if ("1".equals(pattern) || "spiral".equals(pattern) || "螺旋式".equals(pattern) || "螺旋".equals(pattern)) { |
| | | mode = "spiral"; |
| | | } else if ("parallel".equals(pattern) || "平行线".equals(pattern) || "平行".equals(pattern)) { |
| | | mode = "parallel"; |
| | | } |
| | | } |
| | | |
| | | // 调用路径规划生成完整路径段 |
| | | segments = lujing.Lunjingguihua.generatePathSegments( |
| | | boundaryCoords, |
| | | obstaclesCoords != null ? obstaclesCoords : "", |
| | | mowingWidth, |
| | | safetyDistance, |
| | | mode |
| | | ); |
| | | } catch (Exception e) { |
| | | // 如果重新生成失败,segments 为 null |
| | | System.err.println("导航预览重新生成路径失败: " + e.getMessage()); |
| | | } |
| | | } |
| | | |
| | | // 3. 构建最终导航路径点列表 |
| | | pathPoints = new ArrayList<>(); |
| | | if (segments != null && !segments.isEmpty()) { |
| | | // 如果成功生成了路径段,使用路径段构建点列表 |
| | | // 这样包含了围边路径和作业路径,以及它们之间的连接 |
| | | lujing.Lunjingguihua.PathSegment firstSeg = segments.get(0); |
| | | pathPoints.add(new Point2D.Double(firstSeg.start.x, firstSeg.start.y)); |
| | | |
| | | for (lujing.Lunjingguihua.PathSegment seg : segments) { |
| | | // 添加终点(起点已经在上一次循环或初始化时添加了) |
| | | // 注意:这里假设路径段是连续的,或者我们只关心端点 |
| | | // 如果段之间不连续(有空走),generatePathSegments 应该已经生成了连接段(isMowing=false) |
| | | pathPoints.add(new Point2D.Double(seg.end.x, seg.end.y)); |
| | | } |
| | | } else { |
| | | // 如果生成失败,回退到使用原始解析的路径点 |
| | | // 这通常只包含作业路径,可能没有围边 |
| | | pathPoints = rawPathPoints; |
| | | } |
| | | |
| | | if (pathPoints == null || pathPoints.size() < 2) { |
| | | JOptionPane.showMessageDialog(null, "无法构建有效的导航路径", "错误", JOptionPane.ERROR_MESSAGE); |
| | | return; |
| | | } |
| | | |
| | | this.currentDikuai = dikuai; |
| | | this.currentPathIndex = 0; |
| | | this.currentSpeed = DEFAULT_SPEED; |
| | |
| | | import java.util.List; |
| | | |
| | | /** |
| | | * 凸形草地路径规划 (避障优化版) |
| | | * 优化:增加了障碍物区间预处理、路径连接就近原则、以及更稳健的多边形外扩 |
| | | * 凸形草地路径规划 (避障修复版) |
| | | * 修复重点:强化跨行及避障后的路径连贯性,确保每一段切割都有显式的移动路径连接。 |
| | | */ |
| | | public class AoxinglujingHaveObstacel { |
| | | |
| | |
| | | public static class PolygonObstacle extends Obstacle { |
| | | public List<Point> points; |
| | | public PolygonObstacle(List<Point> points) { this.points = points; } |
| | | |
| | | @Override |
| | | public boolean isInside(Point p) { |
| | | boolean result = false; |
| | |
| | | } |
| | | return result; |
| | | } |
| | | |
| | | @Override |
| | | public List<Double> getIntersectionsX(double y, double angle) { |
| | | List<Point> rotated = rotatePolygon(this.points, -angle); |
| | |
| | | public Point center; |
| | | public double radius; |
| | | public CircleObstacle(Point center, double radius) { this.center = center; this.radius = radius; } |
| | | |
| | | @Override |
| | | public boolean isInside(Point p) { |
| | | return Math.hypot(p.x - center.x, p.y - center.y) <= radius + EPSILON; |
| | | } |
| | | |
| | | public boolean isInside(Point p) { return Math.hypot(p.x - center.x, p.y - center.y) <= radius + EPSILON; } |
| | | @Override |
| | | public List<Double> getIntersectionsX(double y, double angle) { |
| | | List<Double> xInts = new ArrayList<>(); |
| | |
| | | double dy = Math.abs(y - rCenter.y); |
| | | if (dy < radius) { |
| | | double dx = Math.sqrt(radius * radius - dy * dy); |
| | | xInts.add(rCenter.x - dx); |
| | | xInts.add(rCenter.x + dx); |
| | | xInts.add(rCenter.x - dx); xInts.add(rCenter.x + dx); |
| | | } |
| | | return xInts; |
| | | } |
| | |
| | | double width = Double.parseDouble(widthStr); |
| | | double margin = Double.parseDouble(marginStr); |
| | | List<Obstacle> obstacles = parseObstacles(obstacleStr, margin); |
| | | |
| | | return planPathCore(boundary, obstacles, width, margin); |
| | | } |
| | | |
| | | private static List<PathSegment> planPathCore(List<Point> boundary, List<Obstacle> obstacles, double width, double margin) { |
| | | if (boundary.size() < 3) return new ArrayList<>(); |
| | | |
| | | ensureCCW(boundary); |
| | | List<Point> workArea = shrinkPolygon(boundary, margin); |
| | | if (workArea.size() < 3) return new ArrayList<>(); |
| | |
| | | List<Point> alignedWorkArea = alignBoundaryToStart(workArea, firstScanStart); |
| | | |
| | | List<PathSegment> finalPath = new ArrayList<>(); |
| | | |
| | | // 1. 围边路径 |
| | | // 1. 添加围边路径 |
| | | for (int i = 0; i < alignedWorkArea.size(); i++) { |
| | | finalPath.add(new PathSegment(alignedWorkArea.get(i), alignedWorkArea.get((i + 1) % alignedWorkArea.size()), true)); |
| | | } |
| | | |
| | | // 2. 内部填充 |
| | | // 2. 生成内部弓字形 (强化连接逻辑) |
| | | Point currentPos = alignedWorkArea.get(0); |
| | | List<PathSegment> zigZagLines = generateOptimizedZigZag(workArea, obstacles, bestAngle, width, currentPos); |
| | | finalPath.addAll(zigZagLines); |
| | | List<PathSegment> zigZag = generateFixedZigZag(workArea, obstacles, bestAngle, width, currentPos); |
| | | finalPath.addAll(zigZag); |
| | | |
| | | return finalPath; |
| | | } |
| | | |
| | | private static List<PathSegment> generateOptimizedZigZag(List<Point> polygon, List<Obstacle> obstacles, double angle, double width, Point startPoint) { |
| | | private static List<PathSegment> generateFixedZigZag(List<Point> polygon, List<Obstacle> obstacles, double angle, double width, Point startPoint) { |
| | | List<PathSegment> result = new ArrayList<>(); |
| | | List<Point> rotatedPoly = rotatePolygon(polygon, -angle); |
| | | |
| | | double minY = Double.MAX_VALUE, maxY = -Double.MAX_VALUE; |
| | | for (Point p : rotatedPoly) { |
| | | minY = Math.min(minY, p.y); maxY = Math.max(maxY, p.y); |
| | | } |
| | | for (Point p : rotatedPoly) { minY = Math.min(minY, p.y); maxY = Math.max(maxY, p.y); } |
| | | |
| | | Point currentPos = startPoint; |
| | | boolean leftToRight = true; |
| | | |
| | | for (double y = minY + width; y < maxY - width / 2; y += width) { |
| | | List<Double> intersections = getXIntersections(rotatedPoly, y); |
| | | if (intersections.size() < 2) continue; |
| | | Collections.sort(intersections); |
| | | List<Double> polyInts = getXIntersections(rotatedPoly, y); |
| | | if (polyInts.size() < 2) continue; |
| | | Collections.sort(polyInts); |
| | | double xMin = polyInts.get(0), xMax = polyInts.get(polyInts.size() - 1); |
| | | |
| | | double xBoundaryMin = intersections.get(0); |
| | | double xBoundaryMax = intersections.get(intersections.size() - 1); |
| | | |
| | | // 收集当前行所有障碍物交点并进行裁剪 |
| | | List<Double> splitPoints = new ArrayList<>(); |
| | | splitPoints.add(xBoundaryMin); |
| | | // 收集所有分割点(边界 + 障碍物交点) |
| | | List<Double> splits = new ArrayList<>(); |
| | | splits.add(xMin); splits.add(xMax); |
| | | for (Obstacle obs : obstacles) { |
| | | List<Double> obsX = obs.getIntersectionsX(y, angle); |
| | | for (double ox : obsX) { |
| | | if (ox > xBoundaryMin && ox < xBoundaryMax) splitPoints.add(ox); |
| | | for (double ox : obs.getIntersectionsX(y, angle)) { |
| | | if (ox > xMin + EPSILON && ox < xMax - EPSILON) splits.add(ox); |
| | | } |
| | | } |
| | | Collections.sort(splitPoints); |
| | | Collections.sort(splits); |
| | | |
| | | // 构建有效段 |
| | | List<LineRange> validRanges = new ArrayList<>(); |
| | | for (int i = 0; i < splitPoints.size() - 1; i++) { |
| | | double midX = (splitPoints.get(i) + splitPoints.get(i + 1)) / 2.0; |
| | | Point midPoint = rotatePoint(new Point(midX, y), angle); |
| | | |
| | | boolean insideAnyObstacle = false; |
| | | for (Obstacle obs : obstacles) { |
| | | if (obs.isInside(midPoint)) { |
| | | insideAnyObstacle = true; |
| | | break; |
| | | } |
| | | } |
| | | if (!insideAnyObstacle) { |
| | | validRanges.add(new LineRange(splitPoints.get(i), splitPoints.get(i+1))); |
| | | // 构建本行候选段 |
| | | List<Double[]> rowSegments = new ArrayList<>(); |
| | | for (int i = 0; i < splits.size() - 1; i++) { |
| | | double s = splits.get(i), e = splits.get(i+1); |
| | | Point mid = rotatePoint(new Point((s + e) / 2.0, y), angle); |
| | | if (!isPointInAnyObstacle(mid, obstacles)) { |
| | | rowSegments.add(new Double[]{s, e}); |
| | | } |
| | | } |
| | | |
| | | // 根据当前朝向排序有效段 |
| | | // 根据当前S型方向排序 |
| | | if (!leftToRight) { |
| | | Collections.reverse(validRanges); |
| | | for (LineRange range : validRanges) { |
| | | double temp = range.start; |
| | | range.start = range.end; |
| | | range.end = temp; |
| | | } |
| | | Collections.reverse(rowSegments); |
| | | for (Double[] seg : rowSegments) { double t = seg[0]; seg[0] = seg[1]; seg[1] = t; } |
| | | } |
| | | |
| | | // 连接路径 |
| | | for (LineRange range : validRanges) { |
| | | Point pStart = rotatePoint(new Point(range.start, y), angle); |
| | | Point pEnd = rotatePoint(new Point(range.end, y), angle); |
| | | // 执行连接:强制检查 currentPos 到每一段起点的距离 |
| | | for (Double[] seg : rowSegments) { |
| | | Point p1 = rotatePoint(new Point(seg[0], y), angle); |
| | | Point p2 = rotatePoint(new Point(seg[1], y), angle); |
| | | |
| | | if (Math.hypot(currentPos.x - pStart.x, currentPos.y - pStart.y) > 0.01) { |
| | | result.add(new PathSegment(currentPos, pStart, false)); |
| | | // 核心修复:无论多近,只要不是同一点,就建立虚线连接,确保路径流转 |
| | | if (dist(currentPos, p1) > 0.001) { |
| | | result.add(new PathSegment(currentPos, p1, false)); |
| | | } |
| | | result.add(new PathSegment(pStart, pEnd, true)); |
| | | currentPos = pEnd; |
| | | result.add(new PathSegment(p1, p2, true)); |
| | | currentPos = p2; |
| | | } |
| | | leftToRight = !leftToRight; |
| | | } |
| | | return result; |
| | | } |
| | | |
| | | private static class LineRange { |
| | | double start, end; |
| | | LineRange(double s, double e) { this.start = s; this.end = e; } |
| | | private static boolean isPointInAnyObstacle(Point p, List<Obstacle> obstacles) { |
| | | for (Obstacle obs : obstacles) if (obs.isInside(p)) return true; |
| | | return false; |
| | | } |
| | | |
| | | // --- 障碍物解析与多边形外扩 --- |
| | | private static double dist(Point p1, Point p2) { |
| | | return Math.hypot(p1.x - p2.x, p1.y - p2.y); |
| | | } |
| | | |
| | | // --- 辅助工具 (解析与变换) --- |
| | | private static List<Obstacle> parseObstacles(String obsStr, double margin) { |
| | | List<Obstacle> list = new ArrayList<>(); |
| | | if (obsStr == null || obsStr.trim().isEmpty()) return list; |
| | | |
| | | if (obsStr == null || obsStr.isEmpty()) return list; |
| | | for (String part : obsStr.split("\\$")) { |
| | | List<Point> pts = parseCoords(part); |
| | | if (pts.size() == 2) { |
| | | double r = Math.hypot(pts.get(0).x - pts.get(1).x, pts.get(0).y - pts.get(1).y); |
| | | double r = dist(pts.get(0), pts.get(1)); |
| | | list.add(new CircleObstacle(pts.get(0), r + margin)); |
| | | } else if (pts.size() > 2) { |
| | | ensureCCW(pts); |
| | |
| | | } |
| | | |
| | | private static List<Point> expandPolygon(List<Point> poly, double margin) { |
| | | List<Point> result = new ArrayList<>(); |
| | | List<Point> res = new ArrayList<>(); |
| | | int n = poly.size(); |
| | | for (int i = 0; i < n; i++) { |
| | | Point pPrev = poly.get((i - 1 + n) % n); |
| | | Point pCurr = poly.get(i); |
| | | Point pNext = poly.get((i + 1) % n); |
| | | |
| | | double d1x = pCurr.x - pPrev.x, d1y = pCurr.y - pPrev.y; |
| | | double l1 = Math.hypot(d1x, d1y); |
| | | double d2x = pNext.x - pCurr.x, d2y = pNext.y - pCurr.y; |
| | | double l2 = Math.hypot(d2x, d2y); |
| | | |
| | | // 计算外法线 |
| | | Point p1 = poly.get((i - 1 + n) % n), p2 = poly.get(i), p3 = poly.get((i + 1) % n); |
| | | double d1x = p2.x - p1.x, d1y = p2.y - p1.y; |
| | | double d2x = p3.x - p2.x, d2y = p3.y - p2.y; |
| | | double l1 = Math.hypot(d1x, d1y), l2 = Math.hypot(d2x, d2y); |
| | | double n1x = d1y / l1, n1y = -d1x / l1; |
| | | double n2x = d2y / l2, n2y = -d2x / l2; |
| | | |
| | | double bx = n1x + n2x, by = n1y + n2y; |
| | | double bLen = Math.hypot(bx, by); |
| | | if (bLen < EPSILON) { bx = n1x; by = n1y; } else { bx /= bLen; by /= bLen; } |
| | | |
| | | double cosHalf = n1x * bx + n1y * by; |
| | | double d = margin / Math.max(cosHalf, 0.1); |
| | | // 限制最大外扩,防止尖角畸变 |
| | | d = Math.min(d, margin * 3); |
| | | result.add(new Point(pCurr.x + bx * d, pCurr.y + by * d)); |
| | | double d = margin / Math.max(n1x * bx + n1y * by, 0.1); |
| | | res.add(new Point(p2.x + bx * Math.min(d, margin * 2), p2.y + by * Math.min(d, margin * 2))); |
| | | } |
| | | return result; |
| | | return res; |
| | | } |
| | | |
| | | // --- 基础工具类方法 --- |
| | | private static List<Point> parseCoords(String s) { |
| | | List<Point> list = new ArrayList<>(); |
| | | if(s == null || s.isEmpty()) return list; |
| | | for (String p : s.split(";")) { |
| | | String[] xy = p.split(","); |
| | | if (xy.length >= 2) list.add(new Point(Double.parseDouble(xy[0]), Double.parseDouble(xy[1]))); |
| | |
| | | if (s > 0) Collections.reverse(poly); |
| | | } |
| | | |
| | | private static List<Point> shrinkPolygon(List<Point> polygon, double margin) { |
| | | List<Point> result = new ArrayList<>(); |
| | | int n = polygon.size(); |
| | | private static List<Point> shrinkPolygon(List<Point> poly, double margin) { |
| | | List<Point> res = new ArrayList<>(); |
| | | int n = poly.size(); |
| | | for (int i = 0; i < n; i++) { |
| | | Point pPrev = polygon.get((i - 1 + n) % n); |
| | | Point pCurr = polygon.get(i); |
| | | Point pNext = polygon.get((i + 1) % n); |
| | | double d1x = pCurr.x - pPrev.x, d1y = pCurr.y - pPrev.y; |
| | | double l1 = Math.hypot(d1x, d1y); |
| | | double d2x = pNext.x - pCurr.x, d2y = pNext.y - pCurr.y; |
| | | double l2 = Math.hypot(d2x, d2y); |
| | | Point p1 = poly.get((i - 1 + n) % n), p2 = poly.get(i), p3 = poly.get((i + 1) % n); |
| | | double d1x = p2.x - p1.x, d1y = p2.y - p1.y; |
| | | double d2x = p3.x - p2.x, d2y = p3.y - p2.y; |
| | | double l1 = Math.hypot(d1x, d1y), l2 = Math.hypot(d2x, d2y); |
| | | double n1x = -d1y / l1, n1y = d1x / l1; |
| | | double n2x = -d2y / l2, n2y = d2x / l2; |
| | | double bx = n1x + n2x, by = n1y + n2y; |
| | | double bLen = Math.hypot(bx, by); |
| | | if (bLen < EPSILON) { bx = n1x; by = n1y; } else { bx /= bLen; by /= bLen; } |
| | | double cosHalf = n1x * bx + n1y * by; |
| | | double d = margin / Math.max(cosHalf, 0.1); |
| | | result.add(new Point(pCurr.x + bx * d, pCurr.y + by * d)); |
| | | double d = margin / Math.max(n1x * bx + n1y * by, 0.1); |
| | | res.add(new Point(p2.x + bx * d, p2.y + by * d)); |
| | | } |
| | | return result; |
| | | return res; |
| | | } |
| | | |
| | | private static double findOptimalScanAngle(List<Point> polygon) { |
| | | private static double findOptimalScanAngle(List<Point> poly) { |
| | | double minH = Double.MAX_VALUE, bestA = 0; |
| | | for (int i = 0; i < polygon.size(); i++) { |
| | | Point p1 = polygon.get(i), p2 = polygon.get((i + 1) % polygon.size()); |
| | | double angle = Math.atan2(p2.y - p1.y, p2.x - p1.x); |
| | | double h = calculatePolygonHeightAtAngle(polygon, angle); |
| | | if (h < minH) { minH = h; bestA = angle; } |
| | | for (int i = 0; i < poly.size(); i++) { |
| | | Point p1 = poly.get(i), p2 = poly.get((i + 1) % poly.size()); |
| | | double a = Math.atan2(p2.y - p1.y, p2.x - p1.x); |
| | | double minY = Double.MAX_VALUE, maxY = -Double.MAX_VALUE; |
| | | double s = Math.sin(-a), c = Math.cos(-a); |
| | | for (Point p : poly) { double ry = p.x * s + p.y * c; minY = Math.min(minY, ry); maxY = Math.max(maxY, ry); } |
| | | if (maxY - minY < minH) { minH = maxY - minY; bestA = a; } |
| | | } |
| | | return bestA; |
| | | } |
| | | |
| | | private static double calculatePolygonHeightAtAngle(List<Point> poly, double angle) { |
| | | double minY = Double.MAX_VALUE, maxY = -Double.MAX_VALUE; |
| | | double sin = Math.sin(-angle), cos = Math.cos(-angle); |
| | | for (Point p : poly) { |
| | | double ry = p.x * sin + p.y * cos; |
| | | minY = Math.min(minY, ry); maxY = Math.max(maxY, ry); |
| | | } |
| | | return maxY - minY; |
| | | } |
| | | |
| | | private static List<Double> getXIntersections(List<Point> rotatedPoly, double y) { |
| | | List<Double> xInts = new ArrayList<>(); |
| | | int n = rotatedPoly.size(); |
| | | for (int i = 0; i < n; i++) { |
| | | Point p1 = rotatedPoly.get(i), p2 = rotatedPoly.get((i + 1) % n); |
| | | for (int i = 0; i < rotatedPoly.size(); i++) { |
| | | Point p1 = rotatedPoly.get(i), p2 = rotatedPoly.get((i + 1) % rotatedPoly.size()); |
| | | if ((p1.y <= y && p2.y > y) || (p2.y <= y && p1.y > y)) { |
| | | xInts.add(p1.x + (y - p1.y) * (p2.x - p1.x) / (p2.y - p1.y)); |
| | | } |
| | |
| | | return xInts; |
| | | } |
| | | |
| | | private static Point getFirstScanStartPoint(List<Point> polygon, double angle, double width) { |
| | | List<Point> rotated = rotatePolygon(polygon, -angle); |
| | | private static Point getFirstScanStartPoint(List<Point> poly, double angle, double width) { |
| | | List<Point> rot = rotatePolygon(poly, -angle); |
| | | double minY = Double.MAX_VALUE; |
| | | for (Point p : rotated) minY = Math.min(minY, p.y); |
| | | double startY = minY + width + EPSILON; |
| | | List<Double> xInts = getXIntersections(rotated, startY); |
| | | if (xInts.isEmpty()) return polygon.get(0); |
| | | Collections.sort(xInts); |
| | | return rotatePoint(new Point(xInts.get(0), startY), angle); |
| | | for (Point p : rot) minY = Math.min(minY, p.y); |
| | | double sy = minY + width + EPSILON; |
| | | List<Double> x = getXIntersections(rot, sy); |
| | | Collections.sort(x); |
| | | return rotatePoint(new Point(x.isEmpty() ? 0 : x.get(0), sy), angle); |
| | | } |
| | | |
| | | private static List<Point> alignBoundaryToStart(List<Point> polygon, Point target) { |
| | | int bestIdx = 0; double minDist = Double.MAX_VALUE; |
| | | for (int i = 0; i < polygon.size(); i++) { |
| | | double d = Math.hypot(polygon.get(i).x - target.x, polygon.get(i).y - target.y); |
| | | if (d < minDist) { minDist = d; bestIdx = i; } |
| | | private static List<Point> alignBoundaryToStart(List<Point> poly, Point target) { |
| | | int idx = 0; double minD = Double.MAX_VALUE; |
| | | for (int i = 0; i < poly.size(); i++) { |
| | | double d = dist(poly.get(i), target); |
| | | if (d < minD) { minD = d; idx = i; } |
| | | } |
| | | List<Point> aligned = new ArrayList<>(); |
| | | for (int i = 0; i < polygon.size(); i++) aligned.add(polygon.get((bestIdx + i) % polygon.size())); |
| | | return aligned; |
| | | List<Point> res = new ArrayList<>(); |
| | | for (int i = 0; i < poly.size(); i++) res.add(poly.get((idx + i) % poly.size())); |
| | | return res; |
| | | } |
| | | |
| | | private static Point rotatePoint(Point p, double angle) { |
| | | double c = Math.cos(angle), s = Math.sin(angle); |
| | | private static Point rotatePoint(Point p, double a) { |
| | | double c = Math.cos(a), s = Math.sin(a); |
| | | return new Point(p.x * c - p.y * s, p.x * s + p.y * c); |
| | | } |
| | | |
| | | private static List<Point> rotatePolygon(List<Point> poly, double angle) { |
| | | private static List<Point> rotatePolygon(List<Point> poly, double a) { |
| | | List<Point> res = new ArrayList<>(); |
| | | for (Point p : poly) res.add(rotatePoint(p, angle)); |
| | | for (Point p : poly) res.add(rotatePoint(p, a)); |
| | | return res; |
| | | } |
| | | } |
| | |
| | | JOptionPane.showMessageDialog(this, "请先生成割草路径", "提示", JOptionPane.INFORMATION_MESSAGE); |
| | | return; |
| | | } |
| | | |
| | | // 更新当前地块对象的属性 |
| | | if (dikuai != null) { |
| | | dikuai.setBaseStationCoordinates(baseStationNormalized); |
| | | dikuai.setBoundaryCoordinates(boundaryNormalized); |
| | | dikuai.setMowingWidth(widthNormalized); |
| | | dikuai.setPlannedPath(pathNormalized); |
| | | dikuai.setObstacleCoordinates(obstacleNormalized); |
| | | } |
| | | |
| | | // 调用回调保存数据 |
| | | if (saveCallback != null) { |