package com.algo.model;
|
|
/**
|
* CTU物理参数配置类
|
* 包含CTU运动的各种物理参数，用于路径规划和碰撞检测
|
*/
|
public class CTUPhysicalConfig {
|
|
/**
|
* 最大移动速度 (米/秒)
|
*/
|
private double maxSpeed;
|
|
/**
|
* 正常移动速度 (米/秒)
|
*/
|
private double normalSpeed;
|
|
/**
|
* 最大加速度 (米/秒²)
|
*/
|
private double maxAcceleration;
|
|
/**
|
* 最大减速度 (米/秒²)
|
*/
|
private double maxDeceleration;
|
|
/**
|
* 转向时间 (秒) - 90度转向所需时间
|
*/
|
private double turnTime90;
|
|
/**
|
* 转向时间 (秒) - 180度转向所需时间
|
*/
|
private double turnTime180;
|
|
/**
|
* CTU之间的最小安全距离 (米)
|
*/
|
private double minSafetyDistance;
|
|
/**
|
* CTU之间的最小跟随距离 (米)
|
*/
|
private double minFollowingDistance;
|
|
/**
|
* CTU的物理长度 (米)
|
*/
|
private double ctuLength;
|
|
/**
|
* CTU的物理宽度 (米)
|
*/
|
private double ctuWidth;
|
|
/**
|
* 启动时间 (秒) - 从静止到正常速度所需时间
|
*/
|
private double startupTime;
|
|
/**
|
* 停止时间 (秒) - 从正常速度到静止所需时间
|
*/
|
private double stopTime;
|
|
/**
|
* 路径点之间的标准距离 (米)
|
*/
|
private double standardPointDistance;
|
|
// 默认构造函数 - 使用推荐的默认值
|
public CTUPhysicalConfig() {
|
this.maxSpeed = 2.0; // 2米/秒
|
this.normalSpeed = 1.5; // 1.5米/秒
|
this.maxAcceleration = 1.0; // 1米/秒²
|
this.maxDeceleration = 1.5; // 1.5米/秒²
|
this.turnTime90 = 2.0; // 90度转向需要2秒
|
this.turnTime180 = 4.0; // 180度转向需要4秒
|
this.minSafetyDistance = 3.0; // 最小安全距离3米
|
this.minFollowingDistance = 2.0; // 最小跟随距离2米
|
this.ctuLength = 1.5; // CTU长度1.5米
|
this.ctuWidth = 1.0; // CTU宽度1米
|
this.startupTime = 1.0; // 启动时间1秒
|
this.stopTime = 1.5; // 停止时间1.5秒
|
this.standardPointDistance = 1.0; // 标准路径点距离1米
|
}
|
|
// 完整参数的构造函数
|
public CTUPhysicalConfig(double maxSpeed, double normalSpeed, double maxAcceleration,
|
double maxDeceleration, double turnTime90, double turnTime180,
|
double minSafetyDistance, double minFollowingDistance,
|
double ctuLength, double ctuWidth, double startupTime,
|
double stopTime, double standardPointDistance) {
|
this.maxSpeed = maxSpeed;
|
this.normalSpeed = normalSpeed;
|
this.maxAcceleration = maxAcceleration;
|
this.maxDeceleration = maxDeceleration;
|
this.turnTime90 = turnTime90;
|
this.turnTime180 = turnTime180;
|
this.minSafetyDistance = minSafetyDistance;
|
this.minFollowingDistance = minFollowingDistance;
|
this.ctuLength = ctuLength;
|
this.ctuWidth = ctuWidth;
|
this.startupTime = startupTime;
|
this.stopTime = stopTime;
|
this.standardPointDistance = standardPointDistance;
|
}
|
|
// Getter和Setter方法
|
public double getMaxSpeed() {
|
return maxSpeed;
|
}
|
|
public void setMaxSpeed(double maxSpeed) {
|
this.maxSpeed = maxSpeed;
|
}
|
|
public double getNormalSpeed() {
|
return normalSpeed;
|
}
|
|
public void setNormalSpeed(double normalSpeed) {
|
this.normalSpeed = normalSpeed;
|
}
|
|
public double getMaxAcceleration() {
|
return maxAcceleration;
|
}
|
|
public void setMaxAcceleration(double maxAcceleration) {
|
this.maxAcceleration = maxAcceleration;
|
}
|
|
public double getMaxDeceleration() {
|
return maxDeceleration;
|
}
|
|
public void setMaxDeceleration(double maxDeceleration) {
|
this.maxDeceleration = maxDeceleration;
|
}
|
|
public double getTurnTime90() {
|
return turnTime90;
|
}
|
|
public void setTurnTime90(double turnTime90) {
|
this.turnTime90 = turnTime90;
|
}
|
|
public double getTurnTime180() {
|
return turnTime180;
|
}
|
|
public void setTurnTime180(double turnTime180) {
|
this.turnTime180 = turnTime180;
|
}
|
|
public double getMinSafetyDistance() {
|
return minSafetyDistance;
|
}
|
|
public void setMinSafetyDistance(double minSafetyDistance) {
|
this.minSafetyDistance = minSafetyDistance;
|
}
|
|
public double getMinFollowingDistance() {
|
return minFollowingDistance;
|
}
|
|
public void setMinFollowingDistance(double minFollowingDistance) {
|
this.minFollowingDistance = minFollowingDistance;
|
}
|
|
public double getCtuLength() {
|
return ctuLength;
|
}
|
|
public void setCtuLength(double ctuLength) {
|
this.ctuLength = ctuLength;
|
}
|
|
public double getCtuWidth() {
|
return ctuWidth;
|
}
|
|
public void setCtuWidth(double ctuWidth) {
|
this.ctuWidth = ctuWidth;
|
}
|
|
public double getStartupTime() {
|
return startupTime;
|
}
|
|
public void setStartupTime(double startupTime) {
|
this.startupTime = startupTime;
|
}
|
|
public double getStopTime() {
|
return stopTime;
|
}
|
|
public void setStopTime(double stopTime) {
|
this.stopTime = stopTime;
|
}
|
|
public double getStandardPointDistance() {
|
return standardPointDistance;
|
}
|
|
public void setStandardPointDistance(double standardPointDistance) {
|
this.standardPointDistance = standardPointDistance;
|
}
|
|
/**
|
* 计算从当前速度加速到目标速度所需的时间
|
*
|
* @param currentSpeed 当前速度
|
* @param targetSpeed 目标速度
|
* @return 所需时间（秒）
|
*/
|
public double getAccelerationTime(double currentSpeed, double targetSpeed) {
|
if (targetSpeed > currentSpeed) {
|
return (targetSpeed - currentSpeed) / maxAcceleration;
|
} else {
|
return (currentSpeed - targetSpeed) / maxDeceleration;
|
}
|
}
|
|
/**
|
* 计算从当前速度加速/减速到目标速度所需的距离
|
*
|
* @param currentSpeed 当前速度
|
* @param targetSpeed 目标速度
|
* @return 所需距离（米）
|
*/
|
public double getAccelerationDistance(double currentSpeed, double targetSpeed) {
|
double time = getAccelerationTime(currentSpeed, targetSpeed);
|
return (currentSpeed + targetSpeed) * time / 2.0;
|
}
|
|
/**
|
* 根据路径点距离计算移动时间
|
*
|
* @param distance 距离（米）
|
* @param currentSpeed 当前速度
|
* @return 移动时间（秒）
|
*/
|
public double getTravelTime(double distance, double currentSpeed) {
|
if (currentSpeed <= 0) {
|
currentSpeed = normalSpeed;
|
}
|
return distance / currentSpeed;
|
}
|
|
/**
|
* 根据方向变化计算转向时间
|
*
|
* @param fromDirection 起始方向角度
|
* @param toDirection 目标方向角度
|
* @return 转向时间（秒）
|
*/
|
public double getTurnTime(String fromDirection, String toDirection) {
|
try {
|
double from = Double.parseDouble(fromDirection);
|
double to = Double.parseDouble(toDirection);
|
|
// 计算角度差
|
double angleDiff = Math.abs(to - from);
|
if (angleDiff > 180) {
|
angleDiff = 360 - angleDiff;
|
}
|
|
// 根据角度差计算转向时间
|
if (angleDiff <= 5) { // 基本无转向
|
return 0.0;
|
} else if (angleDiff <= 90) {
|
return turnTime90 * (angleDiff / 90.0);
|
} else if (angleDiff <= 180) {
|
return turnTime90 + (turnTime180 - turnTime90) * ((angleDiff - 90) / 90.0);
|
} else {
|
return turnTime180;
|
}
|
} catch (NumberFormatException e) {
|
// 如果方向不是数字，返回默认转向时间
|
return turnTime90;
|
}
|
}
|
|
/**
|
* 检查两个CTU之间的距离是否满足安全要求
|
*
|
* @param distance 距离（米）
|
* @param isFollowing 是否为跟随情况
|
* @return true如果距离安全
|
*/
|
public boolean isSafeDistance(double distance, boolean isFollowing) {
|
double minDistance = isFollowing ? minFollowingDistance : minSafetyDistance;
|
return distance >= minDistance;
|
}
|
|
@Override
|
public String toString() {
|
return "CTUPhysicalConfig{" +
|
"maxSpeed=" + maxSpeed +
|
", normalSpeed=" + normalSpeed +
|
", maxAcceleration=" + maxAcceleration +
|
", maxDeceleration=" + maxDeceleration +
|
", turnTime90=" + turnTime90 +
|
", turnTime180=" + turnTime180 +
|
", minSafetyDistance=" + minSafetyDistance +
|
", minFollowingDistance=" + minFollowingDistance +
|
", ctuLength=" + ctuLength +
|
", ctuWidth=" + ctuWidth +
|
", startupTime=" + startupTime +
|
", stopTime=" + stopTime +
|
", standardPointDistance=" + standardPointDistance +
|
'}';
|
}
|
}
|
