更新版本

jianghaiyue

17 小时以前 d808837cd368c3772962be591aa6532bcc0cf3e4

 algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java

@@ -4,12 +4,12 @@
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;
import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;

import java.util.*;

/**
 * A*路径规划器实现
 * 使用3D A*算法进行CTU时空路径规划，支持物理约束
 */
public class AStarPathPlanner implements PathPlanner {

@@ -22,6 +22,11 @@
     * 邻接表
     */
    private final Map<String, List<Map<String, String>>> adjacencyList;

    /**
     * 环境连通性数据
     */
    private final JsonUtils.EnvironmentConnectivity environmentConnectivity;

    /**
     * 时间精度（毫秒）
@@ -39,7 +44,7 @@
    private final int maxSearchDepth = 15000;

    /**
     * 距离缓存 - 优化：缓存距离计算结果
     * 距离缓存
     */
    private final Map<String, Double> distanceCache = new HashMap<>();

@@ -47,6 +52,16 @@
     * 快速路径缓存
     */
    private final Map<String, List<String>> fastPathCache = new HashMap<>();
    
    /**
     * 实际坐标映射 
     */
    private Map<String, double[]> realCoordinateMapping;
    
    /**
     * 路径时间计算器
     */
    private PathTimeCalculator timeCalculator;

    /**
     * 构造函数
@@ -57,13 +72,38 @@
        this.pathMapping = pathMapping;
        this.adjacencyList = new HashMap<>();

        // 直接构建邻接表
        buildAdjacencyList();
        // 加载环境连通性数据
        this.environmentConnectivity = JsonUtils.loadEnvironmentConnectivity("environment.json");

        // 构建环境感知的邻接表
        buildEnvironmentAwareAdjacencyList();
        
        // 加载实际坐标映射
        loadRealCoordinateMapping();
        
        // 初始化时间计算器
        this.timeCalculator = new PathTimeCalculator(pathMapping, realCoordinateMapping);

        // 预计算常用距离
        precomputeCommonDistances();

        System.out.println("A*路径规划器初始化完成，邻接表包含 " + adjacencyList.size() + " 个节点");
    }
    
    /**
     * 加载实际坐标映射
     */
    private void loadRealCoordinateMapping() {
        try {
            this.realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
            if (realCoordinateMapping == null || realCoordinateMapping.isEmpty()) {
                System.out.println("未能加载实际坐标映射，使用网格坐标");
                this.realCoordinateMapping = new HashMap<>();
            }
        } catch (Exception e) {
            System.err.println("加载实际坐标映射失败: " + e.getMessage());
            this.realCoordinateMapping = new HashMap<>();
        }
    }

    @Override
@@ -278,7 +318,7 @@
            }
        }

        // 减少等待选项 - 只在必要时等待（20%概率）
        // 减少等待选项
        if (Math.random() < 0.2) {
            long waitTime = timeResolution;
            long waitUntilTime = current.timePoint + waitTime;
@@ -308,27 +348,61 @@
    /**
     * 时空启发式函数
     *
     * @param coord1         当前坐标
     * @param coord2         目标坐标
     * @param coord1         当前坐标（网格）
     * @param coord2         目标坐标（网格）
     * @param currentTime    当前时间
     * @param physicalConfig 物理配置
     * @return 启发式值
     */
    private double spaceTimeHeuristic(int[] coord1, int[] coord2, long currentTime, CTUPhysicalConfig physicalConfig) {
        // 空间距离
        String pathId1 = findPathIdByCoordinate(coord1);
        String pathId2 = findPathIdByCoordinate(coord2);
        
        if (pathId1 != null && pathId2 != null) {
            double[] realCoord1 = JsonUtils.getRealCoordinate(pathId1, realCoordinateMapping);
            double[] realCoord2 = JsonUtils.getRealCoordinate(pathId2, realCoordinateMapping);
            
            if (realCoord1 != null && realCoord2 != null) {
                // 使用实际距离计算
                double realDistance = CTUPhysicalConfig.calculateRealDistance(realCoord1, realCoord2);
                
                // 时间成本估计（假设平均速度）
                double timeEstimate = realDistance / physicalConfig.getNormalSpeed();
                
                // 考虑转向成本
                double turnPenalty = realDistance > 0.5 ? physicalConfig.getTurnTime90() / 2.0 : 0;
                
                return timeEstimate + turnPenalty;
            }
        }
        
        // 前方案：使用网格坐标
        double spatialDistance = Math.abs(coord1[0] - coord2[0]) + Math.abs(coord1[1] - coord2[1]);

        // 时间成本估计
        double timeEstimate = spatialDistance * physicalConfig.getStandardPointDistance() / physicalConfig.getNormalSpeed();

        // 考虑转向成本
        double turnPenalty = spatialDistance > 1 ? physicalConfig.getTurnTime90() : 0;

        return timeEstimate + turnPenalty;
    }
    
    /**
     * 根据网格坐标查找路径ID
     */
    private String findPathIdByCoordinate(int[] coord) {
        if (coord == null) return null;
        
        for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
            Map<String, Integer> coordMap = entry.getValue();
            if (coordMap != null && 
                coord[0] == coordMap.getOrDefault("x", -1) && 
                coord[1] == coordMap.getOrDefault("y", -1)) {
                return entry.getKey();
            }
        }
        return null;
    }

    /**
     * 计算移动时间
     * 计算精确的移动时间，基于实际坐标和物理参数
     *
     * @param fromCode       起始代码
     * @param toCode         目标代码
@@ -337,14 +411,70 @@
     * @return 移动时间（毫秒）
     */
    private long calculateTravelTime(String fromCode, String toCode, String direction, CTUPhysicalConfig physicalConfig) {
        // 基本移动时间
        // 获取实际坐标
        double[] fromCoord = JsonUtils.getRealCoordinate(fromCode, realCoordinateMapping);
        double[] toCoord = JsonUtils.getRealCoordinate(toCode, realCoordinateMapping);
        
        if (fromCoord == null || toCoord == null) {
            // fallback到网格坐标计算
            return calculateFallbackTravelTime(fromCode, toCode, physicalConfig);
        }
        
        // 计算实际距离（米）
        double realDistance = CTUPhysicalConfig.calculateRealDistance(fromCoord, toCoord);
        
        if (realDistance == 0.0) {
            return 100; // 最小时间100ms
        }
        
        // 假设起始和结束速度（可以根据上下文进一步优化）
        double startSpeed = 0.0; // 假设从静止开始
        double endSpeed = 0.0;   // 假设结束时停止
        double targetSpeed = physicalConfig.getNormalSpeed();
        
        // 计算精确的移动时间
        double movementTime = physicalConfig.calculatePreciseMovementTime(
            realDistance, startSpeed, endSpeed, targetSpeed
        );
        
        // 计算转向时间
        double turnTime = calculateTurnTime(fromCode, toCode, physicalConfig);
        
        return Math.max(100, (long) ((movementTime + turnTime) * 1000));
    }
    
    /**
     * 计算转向时间
     */
    private double calculateTurnTime(String fromCode, String toCode, CTUPhysicalConfig physicalConfig) {
        
        double[] fromCoord = JsonUtils.getRealCoordinate(fromCode, realCoordinateMapping);
        double[] toCoord = JsonUtils.getRealCoordinate(toCode, realCoordinateMapping);
        
        if (fromCoord == null || toCoord == null) {
            return physicalConfig.getTurnTime90() / 4.0; // 平均转向时间
        }
        
        // 计算方向变化
        double dx = toCoord[0] - fromCoord[0];
        double dy = toCoord[1] - fromCoord[1];
        
        // 直线运动无需转向
        if (Math.abs(dx) < 1.0 && Math.abs(dy) < 1.0) {
            return 0.0;
        }
        
        return physicalConfig.getTurnTime90() / 2.0;
    }
    
    /**
     * 备用时间计算方法（当无法获取实际坐标时使用）
     */
    private long calculateFallbackTravelTime(String fromCode, String toCode, CTUPhysicalConfig physicalConfig) {
        double distance = physicalConfig.getStandardPointDistance();
        double speed = physicalConfig.getNormalSpeed();
        long moveTime = (long) ((distance / speed) * 1000);

        // 转向时间
        long turnTime = (long) (physicalConfig.getTurnTime90() * 1000 / 4); // 假设平均转向时间

        long turnTime = (long) (physicalConfig.getTurnTime90() * 1000 / 4);
        return moveTime + turnTime;
    }

@@ -384,14 +514,17 @@
            }

            PathCode pathCode = new PathCode(node.code, direction);

            // 添加时间信息
            // pathCode.setArrivalTime(node.timePoint);

            codeList.add(pathCode);
        }

        return new PlannedPath("", "", codeList);
        PlannedPath plannedPath = new PlannedPath("", "", codeList);
        
        // 使用统一的时间计算器计算精确时间
        long startTime = pathNodes.get(0).timePoint;
        CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
        timeCalculator.calculatePathTiming(plannedPath, startTime, defaultConfig, 0.0);
        
        return plannedPath;
    }

    /**
@@ -472,6 +605,15 @@

    @Override
    public double calculateDistance(String startCode, String endCode) {
        // 优先使用实际坐标
        double[] startRealCoord = JsonUtils.getRealCoordinate(startCode, realCoordinateMapping);
        double[] endRealCoord = JsonUtils.getRealCoordinate(endCode, realCoordinateMapping);
        
        if (startRealCoord != null && endRealCoord != null) {
            return CTUPhysicalConfig.calculateRealDistance(startRealCoord, endRealCoord);
        }
        
        // 前方案：使用网格坐标
        int[] startCoord = JsonUtils.getCoordinate(startCode, pathMapping);
        int[] endCoord = JsonUtils.getCoordinate(endCode, pathMapping);

@@ -493,9 +635,9 @@
    }

    /**
     * 构建邻接表
     * 构建环境感知的邻接表
     */
    private void buildAdjacencyList() {
    private void buildEnvironmentAwareAdjacencyList() {
        // 创建坐标到编号的临时映射
        Map<String, String> tempCoordToCode = new HashMap<>();
        for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
@@ -523,6 +665,12 @@
            int x = coordMap.get("x");
            int y = coordMap.get("y");

            // 环境感知：检查当前节点是否可通行
            if (!environmentConnectivity.isTraversable(x, y)) {
                adjacencyList.put(code, new ArrayList<>()); // 不可通行的节点没有邻居
                continue;
            }

            List<Map<String, String>> neighbors = new ArrayList<>();

            // 检查四个方向的邻居
@@ -532,7 +680,9 @@
                String coordKey = newX + "," + newY;

                String neighborCode = tempCoordToCode.get(coordKey);
                if (neighborCode != null) {
                
                // 环境感知：只有当邻居节点也可通行时才添加连接
                if (neighborCode != null && environmentConnectivity.isTraversable(newX, newY)) {
                    Map<String, String> neighbor = new HashMap<>();
                    neighbor.put("code", neighborCode);
                    neighbor.put("direction", directionAngles[i]);
@@ -542,6 +692,8 @@

            adjacencyList.put(code, neighbors);
        }
        
        System.out.println("环境感知邻接表构建完成，过滤了不可通行的连接");
    }

    /**
@@ -673,7 +825,7 @@
    }

    /**
     * 快速路径规划 - 先尝试简化空间路径规划
     * 快速路径规划 
     * 对于近距离路径，直接返回结果避免复杂的时空计算
     */
    private PlannedPath tryFastPathPlanning(String startCode, String endCode, List<double[]> constraints) {
@@ -722,7 +874,7 @@
        openSet.offer(startNode);
        gScores.put(startCode, 0.0);

        // 简化的约束检查器
        // 约束检查器
        FastConstraintChecker constraintChecker = new FastConstraintChecker(constraints);

        int searchDepth = 0;