修改了导航预览

826220679@qq.com

17 小时以前 f94b1436d7a28c8e28d010b2cb657ab7c064e353

 src/dikuai/daohangyulan.java

@@ -9,9 +9,10 @@
import java.util.ArrayList;
import zhuye.Shouye;
import zhuye.MapRenderer;
import zhuye.buttonset;
import gecaoji.Gecaoji;
import gecaoji.Device;
import gecaoji.lujingdraw;
import publicway.buttonset;

/**
 * 导航预览功能类
@@ -82,16 +83,30 @@
            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;
        }

        // 3. 构建最终导航路径点列表
        // 直接使用解析出来的路径点
        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;
        // 将当前速度(米/秒)转换为km/h写入设备yaw属性
        try {
            double kmh = this.currentSpeed * 3.6;
            Device.getActiveDevice().setYaw(String.valueOf(kmh));
        } catch (Exception ignore) { }
        this.navigationTrack.clear();
        
        // 获取首页和地图渲染器
@@ -363,43 +378,67 @@
            return;
        }
        
        Point2D.Double targetPoint = pathPoints.get(currentPathIndex + 1);
        
        // 计算到目标点的距离
        double dx = targetPoint.x - currentPos.x;
        double dy = targetPoint.y - currentPos.y;
        double distance = Math.hypot(dx, dy);
        
        // 计算每帧移动的距离（米）
        double moveDistance = currentSpeed * (TIMER_INTERVAL_MS / 1000.0);
        
        if (distance <= moveDistance) {
            // 到达目标点，移动到下一个点
            mower.setPosition(targetPoint.x, targetPoint.y);
            navigationTrack.add(new Point2D.Double(targetPoint.x, targetPoint.y));
            mapRenderer.addNavigationPreviewTrackPoint(targetPoint);
        // 到达阈值：当距离小于这个值时，认为已到达目标点（0.05米）
        double arrivalThreshold = 0.05;
        
        // 循环处理，直到移动距离用完或到达路径终点
        double remainingDistance = moveDistance;
        
        while (remainingDistance > 0.001 && currentPathIndex < pathPoints.size() - 1) {
            Point2D.Double targetPoint = pathPoints.get(currentPathIndex + 1);
            
            currentPathIndex++;
            // 计算到目标点的距离
            double dx = targetPoint.x - currentPos.x;
            double dy = targetPoint.y - currentPos.y;
            double distance = Math.hypot(dx, dy);
            
            // 如果还有下一个点，计算方向
            if (currentPathIndex < pathPoints.size() - 1) {
                Point2D.Double nextPoint = pathPoints.get(currentPathIndex + 1);
                double heading = calculateHeading(targetPoint, nextPoint);
                setMowerHeading(heading);
            // 如果距离小于到达阈值，认为已到达目标点，移动到下一个点
            if (distance <= arrivalThreshold) {
                // 到达目标点，移动到精确位置
                mower.setPosition(targetPoint.x, targetPoint.y);
                navigationTrack.add(new Point2D.Double(targetPoint.x, targetPoint.y));
                mapRenderer.addNavigationPreviewTrackPoint(targetPoint);
                
                currentPos = new Point2D.Double(targetPoint.x, targetPoint.y);
                currentPathIndex++;
                
                // 如果还有下一个点，更新方向
                if (currentPathIndex < pathPoints.size() - 1) {
                    Point2D.Double nextPoint = pathPoints.get(currentPathIndex + 1);
                    double heading = calculateHeading(targetPoint, nextPoint);
                    setMowerHeading(heading);
                }
                
                // 继续处理剩余的移动距离
                continue;
            }
        } else {
            
            // 向目标点移动
            // 先更新方向，确保车头朝向目标点
            // 计算本次实际移动的距离（不超过剩余距离和到目标点的距离）
            double actualMoveDistance = Math.min(remainingDistance, distance);
            
            // 更新方向，确保车头朝向目标点
            double heading = calculateHeading(currentPos, targetPoint);
            setMowerHeading(heading);
            
            double ratio = moveDistance / distance;
            // 计算新位置
            double ratio = actualMoveDistance / distance;
            double newX = currentPos.x + dx * ratio;
            double newY = currentPos.y + dy * ratio;
            
            mower.setPosition(newX, newY);
            navigationTrack.add(new Point2D.Double(newX, newY));
            mapRenderer.addNavigationPreviewTrackPoint(new Point2D.Double(newX, newY));
            
            // 更新当前位置和剩余距离
            currentPos = new Point2D.Double(newX, newY);
            remainingDistance -= actualMoveDistance;
            
            // 如果已经到达目标点附近（距离小于阈值），在下次循环中会处理
            // 继续循环处理剩余的移动距离
        }
        
        // 更新速度显示到地图渲染器
@@ -416,9 +455,9 @@
    
    /**
     * 计算两点之间的方向角（度）
     * 图标默认朝上，向右旋转90度车头朝右
     * atan2返回的角度：向右是0度，向上是90度
     * 需要转换为图标旋转角度：向右需要90度，向上需要0度
     * 车辆图标的车头默认是在屏幕正下方（270度）
     * atan2返回的角度：向右是0度，向上是90度，向左是180度，向下是-90度（270度）
     * 需要转换为图标旋转角度，使车头朝向行驶方向
     */
    private double calculateHeading(Point2D.Double from, Point2D.Double to) {
        double dx = to.x - from.x;
@@ -431,13 +470,13 @@
            atan2Angle += 360;
        }
        
        // 图标默认朝上（0度），向右旋转90度车头朝右
        // 所以：运动方向向右（0度）→ 需要旋转90度
        //      运动方向向上（90度）→ 需要旋转0度
        //      运动方向向左（180度）→ 需要旋转270度
        //      运动方向向下（270度）→ 需要旋转180度
        // 公式：heading = (90 - atan2Angle + 360) % 360
        double heading = (90.0 - atan2Angle + 360.0) % 360.0;
        // 图标默认朝下（270度），需要计算旋转角度使车头朝向行驶方向
        // 如果运动方向向上（90度）→ 需要旋转180度（270 - 90 = 180）
        // 如果运动方向向右（0度）→ 需要旋转270度（270 - 0 = 270）
        // 如果运动方向向左（180度）→ 需要旋转90度（270 - 180 = 90）
        // 如果运动方向向下（270度）→ 需要旋转0度（270 - 270 = 0）
        // 公式：heading = (270 - atan2Angle + 360) % 360
        double heading = (270.0 - atan2Angle + 360.0) % 360.0;
        
        return heading;
    }
@@ -456,6 +495,11 @@
     */
    private void speedUp() {
        currentSpeed += SPEED_MULTIPLIER;
        // 同步到设备yaw属性（km/h）
        try {
            double kmh = currentSpeed * 3.6;
            Device.getActiveDevice().setYaw(String.valueOf(kmh));
        } catch (Exception ignore) { }
        updateSpeedDisplay();
    }
    
@@ -469,6 +513,11 @@
                currentSpeed = 0.1;
            }
        }
        // 同步到设备yaw属性（km/h）
        try {
            double kmh = currentSpeed * 3.6;
            Device.getActiveDevice().setYaw(String.valueOf(kmh));
        } catch (Exception ignore) { }
        updateSpeedDisplay();
    }
    
@@ -476,8 +525,11 @@
     * 更新速度显示
     */
    private void updateSpeedDisplay() {
        // 可以在地图上显示当前速度
        // 这里暂时不实现，如果需要可以在MapRenderer中添加速度显示
        // 导航预览下在割草机图标上方实时显示速度（固定像素间距）
        if (mapRenderer != null) {
            mapRenderer.setNavigationPreviewSpeed(currentSpeed);
            mapRenderer.repaint();
        }
    }
    
    /**
@@ -509,6 +561,11 @@
            mapRenderer.setNavigationPreviewSpeed(0.0); // 清除速度显示
            mapRenderer.repaint();
        }

        // 退出时将设备yaw属性重置为0
        try {
            Device.getActiveDevice().setYaw("0");
        } catch (Exception ignore) { }
        
        // 恢复地块管理页面
        // 在清空currentDikuai之前保存地块编号，使用final变量以便在lambda中使用
@@ -546,3 +603,6 @@
        return isNavigating;
    }
}