package com.algo.util;
|
|
import com.algo.model.CTUPhysicalConfig;
|
import com.algo.model.PathCode;
|
import com.algo.model.PlannedPath;
|
|
import java.util.List;
|
import java.util.Map;
|
|
/**
|
* 路径时间计算器
|
* 统一处理路径中每个点的时间计算，确保在各个模块中计算的一致性
|
* 考虑加减速、转向、停留等所有时间因素
|
*/
|
public class PathTimeCalculator {
|
|
/**
|
* 路径映射表（网格坐标）
|
*/
|
private final Map<String, Map<String, Integer>> pathMapping;
|
|
/**
|
* 实际坐标映射表（物理坐标，毫米）
|
*/
|
private final Map<String, double[]> realCoordinateMapping;
|
|
public PathTimeCalculator(Map<String, Map<String, Integer>> pathMapping,
|
Map<String, double[]> realCoordinateMapping) {
|
this.pathMapping = pathMapping;
|
this.realCoordinateMapping = realCoordinateMapping;
|
}
|
|
/**
|
* 为路径计算时间信息
|
*
|
* @param path 规划路径
|
* @param startTime 起始时间（毫秒时间戳）
|
* @param config CTU物理配置
|
* @param initialSpeed 初始速度（米/秒）
|
*/
|
public void calculatePathTiming(PlannedPath path, long startTime,
|
CTUPhysicalConfig config, double initialSpeed) {
|
List<PathCode> codeList = path.getCodeList();
|
if (codeList == null || codeList.isEmpty()) {
|
return;
|
}
|
|
long currentTime = startTime;
|
double currentSpeed = initialSpeed;
|
|
for (int i = 0; i < codeList.size(); i++) {
|
PathCode currentCode = codeList.get(i);
|
|
// 设置到达时间
|
currentCode.setArrivalTime(currentTime);
|
|
// 计算在当前位置的停留时间
|
double stayDuration = calculateStayDuration(currentCode, config);
|
|
// 如果不是最后一个点，计算转向时间
|
if (i < codeList.size() - 1) {
|
PathCode nextCode = codeList.get(i + 1);
|
double turnTime = calculateTurnTime(currentCode, nextCode, config);
|
stayDuration += turnTime;
|
}
|
|
// 设置离开时间
|
long departureTime = currentTime + (long)(stayDuration * 1000);
|
currentCode.setDepartureTime(departureTime);
|
|
// 计算累计时间
|
currentCode.setCumulativeTime(departureTime - startTime);
|
|
// 如果有下一个点，计算移动时间和速度变化
|
if (i < codeList.size() - 1) {
|
PathCode nextCode = codeList.get(i + 1);
|
|
// 计算精确的移动时间（考虑加减速）
|
MovementTimeResult movement = calculatePreciseMovementTime(
|
currentCode, nextCode, currentSpeed, config
|
);
|
|
currentTime = departureTime + movement.travelTime;
|
currentSpeed = movement.endSpeed;
|
}
|
}
|
}
|
|
/**
|
* 计算两点间移动时间
|
*/
|
private MovementTimeResult calculatePreciseMovementTime(PathCode fromCode, PathCode toCode,
|
double startSpeed, CTUPhysicalConfig config) {
|
// 获取实际距离
|
double realDistance = getRealDistance(fromCode.getCode(), toCode.getCode());
|
|
if (realDistance <= 0) {
|
// 备用：使用网格距离
|
realDistance = getGridDistance(fromCode.getCode(), toCode.getCode())
|
* config.getStandardPointDistance();
|
}
|
|
// 确定目标速度（考虑是否为目标点）
|
double targetSpeed = toCode.isTargetPoint() ? 0.0 : config.getNormalSpeed();
|
|
// 使用物理配置的精确计算方法
|
double movementTime = config.calculatePreciseMovementTime(
|
realDistance, startSpeed, targetSpeed, config.getNormalSpeed()
|
);
|
|
return new MovementTimeResult((long)(movementTime * 1000), targetSpeed);
|
}
|
|
/**
|
* 计算在路径点的停留时间
|
*/
|
private double calculateStayDuration(PathCode pathCode, CTUPhysicalConfig config) {
|
double stayTime = 0.1; // 基础停留时间100ms
|
|
// 根据动作类型增加停留时间
|
if (pathCode.getActionType() != null) {
|
switch (pathCode.getActionType()) {
|
case "1": // 取货
|
stayTime += 10.0;
|
break;
|
case "2": // 放货
|
stayTime += 10.0;
|
break;
|
case "3": // 充电
|
stayTime += 600.0;
|
break;
|
default:
|
stayTime += 0.5;
|
break;
|
}
|
} else if (pathCode.isTargetPoint()) {
|
// 目标点停留时间
|
if ("1".equals(pathCode.getPosType())) {
|
stayTime += 10.0; // 取货
|
} else if ("2".equals(pathCode.getPosType())) {
|
stayTime += 10.0; // 放货
|
} else {
|
stayTime += 10.0; // 其他目标点
|
}
|
}
|
|
return stayTime;
|
}
|
|
/**
|
* 计算转向时间
|
*/
|
private double calculateTurnTime(PathCode fromCode, PathCode toCode, CTUPhysicalConfig config) {
|
if (fromCode.getDirection().equals(toCode.getDirection())) {
|
return 0.0;
|
}
|
|
// 使用配置的转向时间计算方法
|
return config.getTurnTime(fromCode.getDirection(), toCode.getDirection());
|
}
|
|
/**
|
* 获取实际物理距离
|
*/
|
private double getRealDistance(String fromCode, String toCode) {
|
if (realCoordinateMapping == null) {
|
return 0.0;
|
}
|
|
double[] fromCoord = realCoordinateMapping.get(fromCode);
|
double[] toCoord = realCoordinateMapping.get(toCode);
|
|
if (fromCoord != null && toCoord != null) {
|
return CTUPhysicalConfig.calculateRealDistance(fromCoord, toCoord);
|
}
|
|
return 0.0;
|
}
|
|
/**
|
* 获取网格距离
|
*/
|
private double getGridDistance(String fromCode, String toCode) {
|
int[] fromCoord = JsonUtils.getCoordinate(fromCode, pathMapping);
|
int[] toCoord = JsonUtils.getCoordinate(toCode, pathMapping);
|
|
if (fromCoord != null && toCoord != null) {
|
return JsonUtils.calculateEuclideanDistance(fromCoord, toCoord);
|
}
|
|
return 1.0; // 默认距离
|
}
|
|
/**
|
* 计算路径中指定位置之后的时间（用于冲突解决）
|
*
|
* @param path 路径
|
* @param fromIndex 开始重新计算的索引
|
* @param config 物理配置
|
*/
|
public void recalculatePathTimingFromIndex(PlannedPath path, int fromIndex, CTUPhysicalConfig config) {
|
List<PathCode> codeList = path.getCodeList();
|
if (codeList == null || fromIndex >= codeList.size()) {
|
return;
|
}
|
|
// 获取重新计算起点的时间和速度
|
PathCode startCode = codeList.get(fromIndex);
|
long startTime = startCode.getDepartureTime() != null ?
|
startCode.getDepartureTime() : startCode.getArrivalTime();
|
|
// 估算当前速度
|
double currentSpeed = config.getNormalSpeed();
|
|
// 从指定位置开始重新计算
|
long currentTime = startTime;
|
|
for (int i = fromIndex + 1; i < codeList.size(); i++) {
|
PathCode currentCode = codeList.get(i);
|
PathCode previousCode = codeList.get(i - 1);
|
|
// 计算移动时间
|
MovementTimeResult movement = calculatePreciseMovementTime(
|
previousCode, currentCode, currentSpeed, config
|
);
|
|
currentTime += movement.travelTime;
|
currentSpeed = movement.endSpeed;
|
|
// 设置到达时间
|
currentCode.setArrivalTime(currentTime);
|
|
// 计算停留时间
|
double stayDuration = calculateStayDuration(currentCode, config);
|
|
// 如果不是最后一个点，增加转向时间
|
if (i < codeList.size() - 1) {
|
PathCode nextCode = codeList.get(i + 1);
|
double turnTime = calculateTurnTime(currentCode, nextCode, config);
|
stayDuration += turnTime;
|
}
|
|
// 设置离开时间
|
long departureTime = currentTime + (long)(stayDuration * 1000);
|
currentCode.setDepartureTime(departureTime);
|
|
// 更新累计时间
|
long pathStartTime = codeList.get(0).getArrivalTime();
|
currentCode.setCumulativeTime(departureTime - pathStartTime);
|
|
currentTime = departureTime;
|
}
|
}
|
|
/**
|
* 计算路径的总执行时间
|
*/
|
public long calculateTotalPathTime(PlannedPath path) {
|
List<PathCode> codeList = path.getCodeList();
|
if (codeList == null || codeList.isEmpty()) {
|
return 0;
|
}
|
|
PathCode firstCode = codeList.get(0);
|
PathCode lastCode = codeList.get(codeList.size() - 1);
|
|
if (firstCode.getArrivalTime() != null && lastCode.getDepartureTime() != null) {
|
return lastCode.getDepartureTime() - firstCode.getArrivalTime();
|
}
|
|
return 0;
|
}
|
|
/**
|
* 检查路径在指定时间窗口是否与占用表冲突
|
*/
|
public boolean isPathTimeConflictFree(PlannedPath path, Map<String, String> occupancyMap) {
|
List<PathCode> codeList = path.getCodeList();
|
if (codeList == null || codeList.isEmpty()) {
|
return true;
|
}
|
|
for (PathCode pathCode : codeList) {
|
if (pathCode.getArrivalTime() == null || pathCode.getDepartureTime() == null) {
|
continue;
|
}
|
|
int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), pathMapping);
|
if (coord == null) continue;
|
|
// 检查整个停留时间段
|
long startTimeSlot = pathCode.getArrivalTime() / 1000;
|
long endTimeSlot = pathCode.getDepartureTime() / 1000;
|
|
for (long timeSlot = startTimeSlot; timeSlot <= endTimeSlot; timeSlot++) {
|
String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
|
if (occupancyMap.containsKey(spaceTimeKey)) {
|
return false;
|
}
|
}
|
}
|
|
return true;
|
}
|
|
/**
|
* 移动时间计算结果
|
*/
|
private static class MovementTimeResult {
|
final long travelTime; // 移动时间（毫秒）
|
final double endSpeed; // 结束速度（米/秒）
|
|
public MovementTimeResult(long travelTime, double endSpeed) {
|
this.travelTime = travelTime;
|
this.endSpeed = endSpeed;
|
}
|
}
|
}
|
