package com.algo.service;
|
|
import com.algo.model.CTUPhysicalConfig;
|
import com.algo.model.PathCode;
|
import com.algo.model.PlannedPath;
|
import com.algo.util.JsonUtils;
|
import com.algo.util.PathTimeCalculator;
|
|
import java.util.*;
|
|
/**
|
* A*路径规划器实现
|
*/
|
public class AStarPathPlanner implements PathPlanner {
|
|
/**
|
* 路径映射表
|
*/
|
private final Map<String, Map<String, Integer>> pathMapping;
|
|
/**
|
* 邻接表
|
*/
|
private final Map<String, List<Map<String, String>>> adjacencyList;
|
|
/**
|
* 环境连通性数据
|
*/
|
private final JsonUtils.EnvironmentConnectivity environmentConnectivity;
|
|
/**
|
* 时间精度（毫秒）
|
*/
|
private final long timeResolution = 1000;
|
|
/**
|
* 最大搜索时间（毫秒）
|
*/
|
private final long maxSearchTime = 30000; // 30秒
|
|
/**
|
* 最大搜索深度
|
*/
|
private final int maxSearchDepth = 15000;
|
|
/**
|
* 距离缓存
|
*/
|
private final Map<String, Double> distanceCache = new HashMap<>();
|
|
/**
|
* 快速路径缓存
|
*/
|
private final Map<String, List<String>> fastPathCache = new HashMap<>();
|
|
/**
|
* 实际坐标映射
|
*/
|
private Map<String, double[]> realCoordinateMapping;
|
|
/**
|
* 路径时间计算器
|
*/
|
private PathTimeCalculator timeCalculator;
|
|
/**
|
* 构造函数
|
*
|
* @param pathMapping 路径映射表
|
*/
|
public AStarPathPlanner(Map<String, Map<String, Integer>> pathMapping) {
|
this.pathMapping = pathMapping;
|
this.adjacencyList = new HashMap<>();
|
|
// 加载环境连通性数据
|
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
|
public PlannedPath planPath(String startCode, String endCode, List<double[]> constraints) {
|
// 先尝试快速路径规划，失败后再使用完整的时空规划
|
PlannedPath fastPath = tryFastPathPlanning(startCode, endCode, constraints);
|
if (fastPath != null) {
|
return fastPath;
|
}
|
return planSpaceTimePath(startCode, endCode, constraints, null, null);
|
}
|
|
@Override
|
public PlannedPath planPath(String startCode, String endCode) {
|
return planPath(startCode, endCode, null);
|
}
|
|
/**
|
* 规划时空路径
|
*
|
* @param startCode 起始路径点ID
|
* @param endCode 目标路径点ID
|
* @param constraints 静态约束条件
|
* @param spaceTimeOccupancyMap 时空占用表
|
* @param physicalConfig CTU物理配置
|
* @return 规划的路径
|
*/
|
public PlannedPath planSpaceTimePath(String startCode, String endCode,
|
List<double[]> constraints,
|
Map<String, String> spaceTimeOccupancyMap,
|
CTUPhysicalConfig physicalConfig) {
|
// 验证输入
|
if (!isValidPathPoint(startCode) || !isValidPathPoint(endCode)) {
|
System.out.println("无效的路径点: " + startCode + " 或 " + endCode);
|
return null;
|
}
|
|
// 起点和终点相同
|
if (startCode.equals(endCode)) {
|
List<PathCode> codeList = new ArrayList<>();
|
codeList.add(new PathCode(startCode, "90"));
|
return new PlannedPath("", "", codeList);
|
}
|
|
// 使用默认物理配置
|
if (physicalConfig == null) {
|
physicalConfig = createDefaultPhysicalConfig();
|
}
|
|
// 获取起点和终点坐标
|
int[] startCoord = JsonUtils.getCoordinate(startCode, pathMapping);
|
int[] endCoord = JsonUtils.getCoordinate(endCode, pathMapping);
|
|
if (startCoord == null || endCoord == null) {
|
System.out.println("无法获取坐标: " + startCode + " 或 " + endCode);
|
return null;
|
}
|
|
System.out.println("开始时空A*搜索: " + startCode + " -> " + endCode);
|
|
// 时空A*算法实现
|
PlannedPath result = spaceTimeAStarSearch(
|
startCode, endCode, startCoord, endCoord,
|
constraints, spaceTimeOccupancyMap, physicalConfig
|
);
|
|
if (result != null) {
|
System.out.println("时空路径规划成功，路径长度: " + result.getCodeList().size());
|
} else {
|
System.out.println("时空路径规划失败");
|
}
|
|
return result;
|
}
|
|
/**
|
* 时空A*搜索算法实现
|
*
|
* @param startCode 起始路径点ID
|
* @param endCode 目标路径点ID
|
* @param startCoord 起始坐标
|
* @param endCoord 目标坐标
|
* @param constraints 约束条件
|
* @param occupancyMap 时空占用表
|
* @param physicalConfig 物理配置
|
* @return 规划的路径
|
*/
|
private PlannedPath spaceTimeAStarSearch(String startCode, String endCode,
|
int[] startCoord, int[] endCoord,
|
List<double[]> constraints,
|
Map<String, String> occupancyMap,
|
CTUPhysicalConfig physicalConfig) {
|
|
// 使用优先队列实现开放列表
|
PriorityQueue<SpaceTimeAStarNode> openSet = new PriorityQueue<>(
|
Comparator.comparingDouble(n -> n.fScore)
|
);
|
Set<String> closedSet = new HashSet<>();
|
Map<String, Double> gScores = new HashMap<>();
|
Map<String, SpaceTimeAStarNode> cameFrom = new HashMap<>();
|
|
// 起始时间
|
long startTime = System.currentTimeMillis();
|
|
// 初始化起始节点
|
SpaceTimeAStarNode startNode = new SpaceTimeAStarNode(
|
startCode, startCoord, startTime, 0,
|
spaceTimeHeuristic(startCoord, endCoord, startTime, physicalConfig)
|
);
|
|
openSet.offer(startNode);
|
String startKey = createSpaceTimeKey(startCode, startTime);
|
gScores.put(startKey, 0.0);
|
|
// 构建约束检查器
|
EnhancedConstraintChecker constraintChecker = new EnhancedConstraintChecker(
|
constraints, occupancyMap, pathMapping, physicalConfig
|
);
|
|
int searchDepth = 0;
|
long searchStartTime = System.currentTimeMillis();
|
|
while (!openSet.isEmpty()) {
|
// 检查搜索时间和深度限制
|
if (System.currentTimeMillis() - searchStartTime > maxSearchTime ||
|
searchDepth > maxSearchDepth) {
|
System.out.println("搜索超时或达到最大深度，返回null");
|
break;
|
}
|
|
SpaceTimeAStarNode current = openSet.poll();
|
String currentKey = createSpaceTimeKey(current.code, current.timePoint);
|
|
if (closedSet.contains(currentKey)) {
|
continue;
|
}
|
|
closedSet.add(currentKey);
|
searchDepth++;
|
|
// 到达目标
|
if (current.code.equals(endCode)) {
|
System.out.println("找到目标，重建路径，搜索深度: " + searchDepth);
|
return reconstructSpaceTimePath(cameFrom, startNode, current, physicalConfig);
|
}
|
|
// 扩展邻居节点
|
expandNeighbors(current, endCoord, openSet, closedSet, gScores, cameFrom,
|
constraintChecker, physicalConfig);
|
}
|
|
System.out.println("未找到有效路径，搜索深度: " + searchDepth);
|
return null; // 无法找到路径
|
}
|
|
/**
|
* 扩展邻居节点
|
*/
|
private void expandNeighbors(SpaceTimeAStarNode current, int[] endCoord,
|
PriorityQueue<SpaceTimeAStarNode> openSet,
|
Set<String> closedSet, Map<String, Double> gScores,
|
Map<String, SpaceTimeAStarNode> cameFrom,
|
EnhancedConstraintChecker constraintChecker,
|
CTUPhysicalConfig physicalConfig) {
|
|
// 获取空间邻居
|
List<Map<String, String>> neighbors = getNeighbors(current.code);
|
|
for (Map<String, String> neighbor : neighbors) {
|
String neighborCode = neighbor.get("code");
|
String direction = neighbor.get("direction");
|
|
// 计算移动到邻居的时间成本
|
long travelTime = calculateTravelTime(current.code, neighborCode, direction, physicalConfig);
|
long arrivalTime = current.timePoint + travelTime;
|
|
// 时间对齐到分辨率
|
arrivalTime = alignToTimeResolution(arrivalTime);
|
|
String neighborKey = createSpaceTimeKey(neighborCode, arrivalTime);
|
|
if (closedSet.contains(neighborKey)) {
|
continue;
|
}
|
|
// 约束检查
|
if (!constraintChecker.isValidMove(current.code, neighborCode,
|
current.timePoint, arrivalTime)) {
|
continue;
|
}
|
|
// 计算G值
|
double gScore = gScores.get(createSpaceTimeKey(current.code, current.timePoint)) +
|
travelTime / 1000.0; // 转换为秒
|
|
// 更新最佳路径
|
if (!gScores.containsKey(neighborKey) || gScore < gScores.get(neighborKey)) {
|
gScores.put(neighborKey, gScore);
|
|
// 获取邻居坐标
|
int[] neighborCoord = JsonUtils.getCoordinate(neighborCode, pathMapping);
|
if (neighborCoord != null) {
|
// 创建邻居节点
|
SpaceTimeAStarNode neighborNode = new SpaceTimeAStarNode(
|
neighborCode, neighborCoord, arrivalTime, gScore,
|
gScore + spaceTimeHeuristic(neighborCoord, endCoord, arrivalTime, physicalConfig)
|
);
|
|
cameFrom.put(neighborKey, current);
|
openSet.offer(neighborNode);
|
}
|
}
|
}
|
|
// 减少等待选项
|
if (Math.random() < 0.2) {
|
long waitTime = timeResolution;
|
long waitUntilTime = current.timePoint + waitTime;
|
String waitKey = createSpaceTimeKey(current.code, waitUntilTime);
|
|
if (!closedSet.contains(waitKey) &&
|
constraintChecker.isValidWait(current.code, current.timePoint, waitUntilTime)) {
|
|
double waitGScore = gScores.get(createSpaceTimeKey(current.code, current.timePoint)) +
|
(waitTime / 1000.0);
|
|
if (!gScores.containsKey(waitKey) || waitGScore < gScores.get(waitKey)) {
|
gScores.put(waitKey, waitGScore);
|
|
SpaceTimeAStarNode waitNode = new SpaceTimeAStarNode(
|
current.code, current.coordinates, waitUntilTime, waitGScore,
|
waitGScore + spaceTimeHeuristic(current.coordinates, endCoord, waitUntilTime, physicalConfig)
|
);
|
|
cameFrom.put(waitKey, current);
|
openSet.offer(waitNode);
|
}
|
}
|
}
|
}
|
|
/**
|
* 时空启发式函数
|
*
|
* @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 目标代码
|
* @param direction 移动方向
|
* @param physicalConfig 物理配置
|
* @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);
|
return moveTime + turnTime;
|
}
|
|
/**
|
* 重建时空路径
|
*
|
* @param cameFrom 路径来源映射
|
* @param startNode 起始节点
|
* @param endNode 结束节点
|
* @param physicalConfig 物理配置
|
* @return 重建的路径
|
*/
|
private PlannedPath reconstructSpaceTimePath(Map<String, SpaceTimeAStarNode> cameFrom,
|
SpaceTimeAStarNode startNode,
|
SpaceTimeAStarNode endNode,
|
CTUPhysicalConfig physicalConfig) {
|
List<SpaceTimeAStarNode> pathNodes = new ArrayList<>();
|
SpaceTimeAStarNode current = endNode;
|
|
// 反向重建路径
|
while (current != null) {
|
pathNodes.add(0, current);
|
String currentKey = createSpaceTimeKey(current.code, current.timePoint);
|
current = cameFrom.get(currentKey);
|
}
|
|
// 转换为PathCode列表
|
List<PathCode> codeList = new ArrayList<>();
|
for (int i = 0; i < pathNodes.size(); i++) {
|
SpaceTimeAStarNode node = pathNodes.get(i);
|
String direction = "90"; // 默认方向
|
|
// 计算方向
|
if (i < pathNodes.size() - 1) {
|
SpaceTimeAStarNode nextNode = pathNodes.get(i + 1);
|
direction = calculateDirection(node.code, nextNode.code);
|
}
|
|
PathCode pathCode = new PathCode(node.code, direction);
|
codeList.add(pathCode);
|
}
|
|
PlannedPath plannedPath = new PlannedPath("", "", codeList);
|
|
// 使用统一的时间计算器计算精确时间
|
long startTime = pathNodes.get(0).timePoint;
|
CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
|
timeCalculator.calculatePathTiming(plannedPath, startTime, defaultConfig, 0.0);
|
|
return plannedPath;
|
}
|
|
/**
|
* 创建时空键
|
*
|
* @param code 位置代码
|
* @param timePoint 时间点
|
* @return 时空键
|
*/
|
private String createSpaceTimeKey(String code, long timePoint) {
|
return code + "_T" + timePoint;
|
}
|
|
/**
|
* 时间对齐到分辨率
|
*
|
* @param timePoint 时间点
|
* @return 对齐后的时间
|
*/
|
private long alignToTimeResolution(long timePoint) {
|
return (timePoint / timeResolution) * timeResolution;
|
}
|
|
/**
|
* 创建默认物理配置
|
*
|
* @return 默认配置
|
*/
|
private CTUPhysicalConfig createDefaultPhysicalConfig() {
|
CTUPhysicalConfig config = new CTUPhysicalConfig();
|
config.setMaxSpeed(2.0);
|
config.setNormalSpeed(1.5);
|
config.setMaxAcceleration(1.0);
|
config.setMaxDeceleration(1.5);
|
config.setTurnTime90(2.0);
|
config.setTurnTime180(4.0);
|
config.setMinSafetyDistance(3.0);
|
config.setMinFollowingDistance(2.0);
|
config.setCtuLength(1.2);
|
config.setCtuWidth(0.8);
|
config.setStartupTime(1.0);
|
config.setStopTime(0.5);
|
config.setStandardPointDistance(1.0);
|
return config;
|
}
|
|
/**
|
* 计算移动方向
|
*
|
* @param fromCode 起始路径点
|
* @param toCode 目标路径点
|
* @return 方向角度字符串
|
*/
|
private String calculateDirection(String fromCode, String toCode) {
|
int[] fromCoord = JsonUtils.getCoordinate(fromCode, pathMapping);
|
int[] toCoord = JsonUtils.getCoordinate(toCode, pathMapping);
|
|
if (fromCoord == null || toCoord == null) {
|
return "90";
|
}
|
|
int dx = toCoord[0] - fromCoord[0];
|
int dy = toCoord[1] - fromCoord[1];
|
|
// 确定方向
|
if (dx == 0 && dy == -1) return "270"; // 上
|
if (dx == 1 && dy == 0) return "0"; // 右
|
if (dx == 0 && dy == 1) return "90"; // 下
|
if (dx == -1 && dy == 0) return "180"; // 左
|
|
return "90"; // 默认方向
|
}
|
|
@Override
|
public boolean isValidPathPoint(String pathCode) {
|
return pathCode != null && pathMapping.containsKey(pathCode);
|
}
|
|
@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);
|
|
if (startCoord == null || endCoord == null) {
|
return -1;
|
}
|
|
return JsonUtils.calculateManhattanDistance(startCoord, endCoord);
|
}
|
|
@Override
|
public Map<String, Map<String, Integer>> getPathMapping() {
|
return pathMapping;
|
}
|
|
@Override
|
public List<Map<String, String>> getNeighbors(String pathCode) {
|
return adjacencyList.getOrDefault(pathCode, new ArrayList<>());
|
}
|
|
/**
|
* 构建环境感知的邻接表
|
*/
|
private void buildEnvironmentAwareAdjacencyList() {
|
// 创建坐标到编号的临时映射
|
Map<String, String> tempCoordToCode = new HashMap<>();
|
for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
|
String code = entry.getKey();
|
Map<String, Integer> coordMap = entry.getValue();
|
|
if (coordMap.containsKey("x") && coordMap.containsKey("y")) {
|
String coordKey = coordMap.get("x") + "," + coordMap.get("y");
|
tempCoordToCode.put(coordKey, code);
|
}
|
}
|
|
// 四个方向：上、右、下、左
|
int[][] directions = {{0, -1}, {1, 0}, {0, 1}, {-1, 0}};
|
String[] directionAngles = {"270", "0", "90", "180"};
|
|
for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
|
String code = entry.getKey();
|
Map<String, Integer> coordMap = entry.getValue();
|
|
if (!coordMap.containsKey("x") || !coordMap.containsKey("y")) {
|
continue;
|
}
|
|
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<>();
|
|
// 检查四个方向的邻居
|
for (int i = 0; i < directions.length; i++) {
|
int newX = x + directions[i][0];
|
int newY = y + directions[i][1];
|
String coordKey = newX + "," + newY;
|
|
String neighborCode = tempCoordToCode.get(coordKey);
|
|
// 环境感知：只有当邻居节点也可通行时才添加连接
|
if (neighborCode != null && environmentConnectivity.isTraversable(newX, newY)) {
|
Map<String, String> neighbor = new HashMap<>();
|
neighbor.put("code", neighborCode);
|
neighbor.put("direction", directionAngles[i]);
|
neighbors.add(neighbor);
|
}
|
}
|
|
adjacencyList.put(code, neighbors);
|
}
|
|
System.out.println("环境感知邻接表构建完成，过滤了不可通行的连接");
|
}
|
|
/**
|
* 时空A*节点类
|
*/
|
private static class SpaceTimeAStarNode {
|
final String code;
|
final int[] coordinates;
|
final long timePoint;
|
final double gScore;
|
final double fScore;
|
|
public SpaceTimeAStarNode(String code, int[] coordinates, long timePoint,
|
double gScore, double fScore) {
|
this.code = code;
|
this.coordinates = coordinates;
|
this.timePoint = timePoint;
|
this.gScore = gScore;
|
this.fScore = fScore;
|
}
|
}
|
|
/**
|
* 约束检查器
|
*/
|
private static class EnhancedConstraintChecker {
|
private final List<double[]> staticConstraints;
|
private final Map<String, String> spaceTimeOccupancyMap;
|
private final Map<String, Map<String, Integer>> pathMapping;
|
private final CTUPhysicalConfig physicalConfig;
|
|
public EnhancedConstraintChecker(List<double[]> staticConstraints,
|
Map<String, String> spaceTimeOccupancyMap,
|
Map<String, Map<String, Integer>> pathMapping,
|
CTUPhysicalConfig physicalConfig) {
|
this.staticConstraints = staticConstraints;
|
this.spaceTimeOccupancyMap = spaceTimeOccupancyMap != null ? spaceTimeOccupancyMap : new HashMap<>();
|
this.pathMapping = pathMapping;
|
this.physicalConfig = physicalConfig;
|
}
|
|
/**
|
* 检查移动是否有效
|
*/
|
public boolean isValidMove(String fromCode, String toCode, long fromTime, long toTime) {
|
// 检查静态约束
|
if (!isValidStaticConstraint(toCode)) {
|
return false;
|
}
|
|
// 检查时空占用
|
if (!isValidSpaceTimeConstraint(toCode, toTime)) {
|
return false;
|
}
|
|
// 检查物理约束
|
return isValidPhysicalConstraint(fromCode, toCode, fromTime, toTime);
|
}
|
|
/**
|
* 检查等待是否有效
|
*/
|
public boolean isValidWait(String code, long fromTime, long toTime) {
|
// 检查时空占用期间是否有冲突
|
for (long t = fromTime; t <= toTime; t += 100) { // 100ms检查间隔
|
if (!isValidSpaceTimeConstraint(code, t)) {
|
return false;
|
}
|
}
|
return true;
|
}
|
|
private boolean isValidStaticConstraint(String pathCode) {
|
if (staticConstraints == null || staticConstraints.isEmpty()) {
|
return true;
|
}
|
|
int[] coord = JsonUtils.getCoordinate(pathCode, pathMapping);
|
if (coord == null) {
|
return false;
|
}
|
|
for (double[] constraint : staticConstraints) {
|
double cx = constraint[0];
|
double cy = constraint[1];
|
double radius = constraint[2];
|
|
double distance = Math.sqrt(Math.pow(coord[0] - cx, 2) + Math.pow(coord[1] - cy, 2));
|
if (distance <= radius) {
|
return false;
|
}
|
}
|
|
return true;
|
}
|
|
private boolean isValidSpaceTimeConstraint(String pathCode, long timePoint) {
|
int[] coord = JsonUtils.getCoordinate(pathCode, pathMapping);
|
if (coord == null) {
|
return false;
|
}
|
|
// 检查精确的时空占用
|
String spaceTimeKey = coord[0] + "," + coord[1] + "," + (timePoint / 1000);
|
if (spaceTimeOccupancyMap.containsKey(spaceTimeKey)) {
|
return false;
|
}
|
|
// 检查周围时间段的占用（考虑安全间隔）
|
long safetyMargin = (long) (physicalConfig.getMinSafetyDistance() * 1000 / physicalConfig.getNormalSpeed());
|
for (long t = timePoint - safetyMargin; t <= timePoint + safetyMargin; t += 100) {
|
String key = coord[0] + "," + coord[1] + "," + (t / 1000);
|
if (spaceTimeOccupancyMap.containsKey(key)) {
|
return false;
|
}
|
}
|
|
return true;
|
}
|
|
private boolean isValidPhysicalConstraint(String fromCode, String toCode, long fromTime, long toTime) {
|
// 物理约束检查
|
long timeDiff = toTime - fromTime;
|
double minTimeRequired = physicalConfig.getStandardPointDistance() / physicalConfig.getMaxSpeed() * 1000;
|
|
// 时间不能小于物理最小时间
|
return timeDiff >= minTimeRequired;
|
}
|
}
|
|
/**
|
* 快速路径规划
|
* 对于近距离路径，直接返回结果避免复杂的时空计算
|
*/
|
private PlannedPath tryFastPathPlanning(String startCode, String endCode, List<double[]> constraints) {
|
// 检查缓存
|
String cacheKey = startCode + "->" + endCode;
|
if (fastPathCache.containsKey(cacheKey)) {
|
List<String> cachedPath = fastPathCache.get(cacheKey);
|
return convertToPlannedPath(cachedPath);
|
}
|
|
// 计算距离，如果距离较近则使用快速算法
|
double distance = getCachedDistance(startCode, endCode);
|
if (distance > 0 && distance <= 40) { // 40个单位以内使用快速算法
|
List<String> fastPath = simpleAStarSearch(startCode, endCode, constraints);
|
if (fastPath != null && !fastPath.isEmpty()) {
|
fastPathCache.put(cacheKey, fastPath); // 缓存结果
|
return convertToPlannedPath(fastPath);
|
}
|
}
|
|
return null; // 快速路径失败，需要使用完整算法
|
}
|
|
/**
|
* 简化A*搜索
|
*/
|
private List<String> simpleAStarSearch(String startCode, String endCode, List<double[]> constraints) {
|
int[] startCoord = JsonUtils.getCoordinate(startCode, pathMapping);
|
int[] endCoord = JsonUtils.getCoordinate(endCode, pathMapping);
|
|
if (startCoord == null || endCoord == null) {
|
return null;
|
}
|
|
PriorityQueue<SimpleAStarNode> openSet = new PriorityQueue<>(
|
Comparator.comparingDouble(n -> n.fScore)
|
);
|
Set<String> closedSet = new HashSet<>();
|
Map<String, Double> gScores = new HashMap<>();
|
Map<String, SimpleAStarNode> cameFrom = new HashMap<>();
|
|
SimpleAStarNode startNode = new SimpleAStarNode(
|
startCode, startCoord, 0, spatialHeuristic(startCoord, endCoord)
|
);
|
|
openSet.offer(startNode);
|
gScores.put(startCode, 0.0);
|
|
// 约束检查器
|
FastConstraintChecker constraintChecker = new FastConstraintChecker(constraints);
|
|
int searchDepth = 0;
|
long searchStartTime = System.currentTimeMillis();
|
|
while (!openSet.isEmpty() && searchDepth < 3000) {
|
if (System.currentTimeMillis() - searchStartTime > 5000) { // 5秒超时
|
break;
|
}
|
|
SimpleAStarNode current = openSet.poll();
|
|
if (closedSet.contains(current.code)) {
|
continue;
|
}
|
|
closedSet.add(current.code);
|
searchDepth++;
|
|
if (current.code.equals(endCode)) {
|
return reconstructSimplePath(cameFrom, startNode, current);
|
}
|
|
// 扩展邻居
|
expandSimpleNeighbors(current, endCoord, openSet, closedSet, gScores, cameFrom, constraintChecker);
|
}
|
|
return null;
|
}
|
|
/**
|
* 预计算常用距离
|
*/
|
private void precomputeCommonDistances() {
|
System.out.println("距离缓存初始化完成");
|
}
|
|
/**
|
* 获取缓存的距离
|
*/
|
private double getCachedDistance(String from, String to) {
|
String key = from + "-" + to;
|
return distanceCache.computeIfAbsent(key, k -> {
|
int[] coord1 = JsonUtils.getCoordinate(from, pathMapping);
|
int[] coord2 = JsonUtils.getCoordinate(to, pathMapping);
|
if (coord1 != null && coord2 != null) {
|
return JsonUtils.calculateManhattanDistance(coord1, coord2);
|
}
|
return -1.0;
|
});
|
}
|
|
/**
|
* 空间启发式函数
|
*/
|
private double spatialHeuristic(int[] coord1, int[] coord2) {
|
return Math.abs(coord1[0] - coord2[0]) + Math.abs(coord1[1] - coord2[1]);
|
}
|
|
/**
|
* 扩展邻居节点
|
*/
|
private void expandSimpleNeighbors(SimpleAStarNode current, int[] endCoord,
|
PriorityQueue<SimpleAStarNode> openSet,
|
Set<String> closedSet, Map<String, Double> gScores,
|
Map<String, SimpleAStarNode> cameFrom,
|
FastConstraintChecker constraintChecker) {
|
|
List<Map<String, String>> neighbors = getNeighbors(current.code);
|
|
for (Map<String, String> neighbor : neighbors) {
|
String neighborCode = neighbor.get("code");
|
|
if (closedSet.contains(neighborCode)) {
|
continue;
|
}
|
|
if (!constraintChecker.isValidPosition(neighborCode)) {
|
continue;
|
}
|
|
double edgeCost = getCachedDistance(current.code, neighborCode);
|
if (edgeCost < 0) edgeCost = 1.0; // 默认边权重
|
|
double gScore = gScores.get(current.code) + edgeCost;
|
|
if (!gScores.containsKey(neighborCode) || gScore < gScores.get(neighborCode)) {
|
gScores.put(neighborCode, gScore);
|
|
int[] neighborCoord = JsonUtils.getCoordinate(neighborCode, pathMapping);
|
if (neighborCoord != null) {
|
SimpleAStarNode neighborNode = new SimpleAStarNode(
|
neighborCode, neighborCoord, gScore,
|
gScore + spatialHeuristic(neighborCoord, endCoord)
|
);
|
|
cameFrom.put(neighborCode, current);
|
openSet.offer(neighborNode);
|
}
|
}
|
}
|
}
|
|
/**
|
* 重建简化路径
|
*/
|
private List<String> reconstructSimplePath(Map<String, SimpleAStarNode> cameFrom,
|
SimpleAStarNode startNode, SimpleAStarNode endNode) {
|
List<String> path = new ArrayList<>();
|
SimpleAStarNode current = endNode;
|
|
while (current != null) {
|
path.add(0, current.code);
|
current = cameFrom.get(current.code);
|
}
|
|
return path;
|
}
|
|
/**
|
* 将路径转换为PlannedPath
|
*/
|
private PlannedPath convertToPlannedPath(List<String> pathCodes) {
|
if (pathCodes == null || pathCodes.isEmpty()) {
|
return null;
|
}
|
|
List<PathCode> codeList = new ArrayList<>();
|
|
for (int i = 0; i < pathCodes.size(); i++) {
|
String position = pathCodes.get(i);
|
String direction = "90"; // 默认方向
|
|
// 计算方向
|
if (i < pathCodes.size() - 1) {
|
direction = calculateDirection(position, pathCodes.get(i + 1));
|
}
|
|
PathCode pathCode = new PathCode(position, direction);
|
codeList.add(pathCode);
|
}
|
|
return new PlannedPath("", "", codeList);
|
}
|
|
/**
|
* A*节点类
|
*/
|
private static class SimpleAStarNode {
|
final String code;
|
final int[] coordinates;
|
final double gScore;
|
final double fScore;
|
|
public SimpleAStarNode(String code, int[] coordinates, double gScore, double fScore) {
|
this.code = code;
|
this.coordinates = coordinates;
|
this.gScore = gScore;
|
this.fScore = fScore;
|
}
|
}
|
|
/**
|
* 约束检查器
|
*/
|
private class FastConstraintChecker {
|
private final Set<String> blockedPositions;
|
|
public FastConstraintChecker(List<double[]> constraints) {
|
this.blockedPositions = new HashSet<>();
|
precomputeBlockedPositions(constraints);
|
}
|
|
private void precomputeBlockedPositions(List<double[]> constraints) {
|
if (constraints == null || constraints.isEmpty()) {
|
return;
|
}
|
|
for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
|
String pathCode = entry.getKey();
|
int[] coord = JsonUtils.getCoordinate(pathCode, pathMapping);
|
|
if (coord != null && isBlocked(coord, constraints)) {
|
blockedPositions.add(pathCode);
|
}
|
}
|
}
|
|
private boolean isBlocked(int[] coord, List<double[]> constraints) {
|
for (double[] constraint : constraints) {
|
double cx = constraint[0];
|
double cy = constraint[1];
|
double radius = constraint[2];
|
|
double distance = Math.sqrt(Math.pow(coord[0] - cx, 2) + Math.pow(coord[1] - cy, 2));
|
if (distance <= radius) {
|
return true;
|
}
|
}
|
return false;
|
}
|
|
public boolean isValidPosition(String pathCode) {
|
return !blockedPositions.contains(pathCode);
|
}
|
}
|
}
|
