完善了碰撞检测算法
This commit is contained in:
parent
af458a0b16
commit
3c3b4190b8
@ -81,29 +81,27 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
// 检测可控车辆与航空器的碰撞
|
||||
for (const auto& aircraft : aircraftData_) {
|
||||
for (const auto& vehicle : controlVehicles) {
|
||||
|
||||
Logger::debug("检查车辆与航空器碰撞: ", vehicle.vehicleNo, " 与 ", aircraft.flightNo);
|
||||
// 计算当前距离
|
||||
double dx = aircraft.position.x - vehicle.position.x;
|
||||
double dy = aircraft.position.y - vehicle.position.y;
|
||||
double currentDistance = std::sqrt(dx*dx + dy*dy);
|
||||
// 检测碰撞
|
||||
bool hasCollision = checkRectangleBasedAircraftVehicleCollision(
|
||||
aircraft, vehicle, predictionConfig.time_window);
|
||||
|
||||
// 计算预测阈值
|
||||
double maxSpeed = std::max(aircraft.speed, vehicle.speed);
|
||||
double predictionDistance = maxSpeed * predictionConfig.time_window;
|
||||
double baseThreshold = getCollisionParams(aircraft.position).alert_zone_radius;
|
||||
double predictionThreshold = std::max(predictionDistance, baseThreshold * 4.0);
|
||||
|
||||
// 如果在预测距离内，进行轨迹预测
|
||||
if (currentDistance <= predictionThreshold) {
|
||||
// 获取预测结果
|
||||
auto prediction = predictTrajectoryCollision(
|
||||
aircraft.position, aircraft.speed, aircraft.heading,
|
||||
vehicle.position, vehicle.speed, vehicle.heading,
|
||||
predictionConfig.aircraft_size,
|
||||
predictionConfig.vehicle_size,
|
||||
predictionConfig.time_window
|
||||
);
|
||||
if (hasCollision) {
|
||||
// 计算当前距离
|
||||
double dx = aircraft.position.x - vehicle.position.x;
|
||||
double dy = aircraft.position.y - vehicle.position.y;
|
||||
double currentDistance = std::sqrt(dx*dx + dy*dy);
|
||||
|
||||
// 计算相对运动
|
||||
MovementVector av(aircraft.speed, aircraft.heading);
|
||||
MovementVector vv(vehicle.speed, vehicle.heading);
|
||||
double vx = av.vx - vv.vx;
|
||||
double vy = av.vy - vv.vy;
|
||||
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
|
||||
|
||||
// 计算相对运动方向（点积），如果大于0表示正在远离
|
||||
double relativeDirection = (dx*vx + dy*vy) / (currentDistance * relativeSpeed);
|
||||
// 如果正在远离，相对速度为正；如果正在接近，相对速度为负
|
||||
relativeSpeed = relativeSpeed * (relativeDirection > 0 ? 1 : -1);
|
||||
|
||||
// 评估风险等级
|
||||
auto [level, zoneType] = evaluateRisk(
|
||||
@ -111,30 +109,17 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
aircraft.position,
|
||||
true, // aircraft
|
||||
false, // vehicle
|
||||
prediction.willCollide
|
||||
true // hasCollision
|
||||
);
|
||||
|
||||
// 如果存在风险，添加到结果中
|
||||
if (level != RiskLevel::NONE) {
|
||||
// 计算相对运动
|
||||
MovementVector av(aircraft.speed, aircraft.heading);
|
||||
MovementVector vv(vehicle.speed, vehicle.heading);
|
||||
double vx = av.vx - vv.vx;
|
||||
double vy = av.vy - vv.vy;
|
||||
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
|
||||
|
||||
// 计算相对运动方向（点积），如果大于0表示正在远离
|
||||
double relativeDirection = (dx*vx + dy*vy) / (currentDistance * relativeSpeed);
|
||||
// 如果正在远离，相对速度为正；如果正在接近，相对速度为负
|
||||
relativeSpeed = relativeSpeed * (relativeDirection > 0 ? 1 : -1);
|
||||
|
||||
risks.push_back({
|
||||
aircraft.flightNo,
|
||||
vehicle.vehicleNo,
|
||||
level,
|
||||
currentDistance,
|
||||
prediction.minDistance,
|
||||
relativeSpeed, // 带方向的相对速度
|
||||
currentDistance, // 最小距离就是当前距离
|
||||
relativeSpeed,
|
||||
{vx, vy}, // 相对运动向量
|
||||
zoneType
|
||||
});
|
||||
@ -158,33 +143,30 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
for (const auto& vehicle2 : allVehicles) {
|
||||
// 跳过自身
|
||||
if (vehicle1.vehicleNo == vehicle2.vehicleNo) {
|
||||
Logger::debug("跳过自身车辆: ", vehicle1.vehicleNo);
|
||||
continue;
|
||||
}
|
||||
|
||||
Logger::debug("检查车辆碰撞: ", vehicle1.vehicleNo, " 与 ", vehicle2.vehicleNo);
|
||||
// 检测碰撞
|
||||
bool hasCollision = checkRectangleBasedVehicleCollision(
|
||||
vehicle1, vehicle2, predictionConfig.time_window);
|
||||
|
||||
// 计算当前距离
|
||||
double dx = vehicle1.position.x - vehicle2.position.x;
|
||||
double dy = vehicle1.position.y - vehicle2.position.y;
|
||||
double currentDistance = std::sqrt(dx*dx + dy*dy);
|
||||
|
||||
// 计算预测阈值
|
||||
double maxSpeed = std::max(vehicle1.speed, vehicle2.speed);
|
||||
double predictionDistance = maxSpeed * predictionConfig.time_window;
|
||||
double baseThreshold = getCollisionParams(vehicle1.position).alert_zone_radius;
|
||||
double predictionThreshold = std::max(predictionDistance, baseThreshold * 4.0);
|
||||
|
||||
// 如果在预测距离内，进行轨迹预测
|
||||
if (currentDistance <= predictionThreshold) {
|
||||
// 获取预测结果
|
||||
auto prediction = predictTrajectoryCollision(
|
||||
vehicle1.position, vehicle1.speed, vehicle1.heading,
|
||||
vehicle2.position, vehicle2.speed, vehicle2.heading,
|
||||
predictionConfig.vehicle_size,
|
||||
predictionConfig.vehicle_size,
|
||||
predictionConfig.time_window
|
||||
);
|
||||
if (hasCollision) {
|
||||
// 计算当前距离
|
||||
double dx = vehicle1.position.x - vehicle2.position.x;
|
||||
double dy = vehicle1.position.y - vehicle2.position.y;
|
||||
double currentDistance = std::sqrt(dx*dx + dy*dy);
|
||||
|
||||
// 计算相对运动
|
||||
MovementVector v1v(vehicle1.speed, vehicle1.heading);
|
||||
MovementVector v2v(vehicle2.speed, vehicle2.heading);
|
||||
double vx = v1v.vx - v2v.vx;
|
||||
double vy = v1v.vy - v2v.vy;
|
||||
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
|
||||
|
||||
// 计算相对运动方向（点积），如果大于0表示正在远离
|
||||
double relativeDirection = (dx*vx + dy*vy) / (currentDistance * relativeSpeed);
|
||||
// 如果正在远离，相对速度为正；如果正在接近，相对速度为负
|
||||
relativeSpeed = relativeSpeed * (relativeDirection > 0 ? 1 : -1);
|
||||
|
||||
// 评估风险等级
|
||||
auto [level, zoneType] = evaluateRisk(
|
||||
@ -192,55 +174,32 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
vehicle1.position,
|
||||
false, // vehicle
|
||||
false, // vehicle
|
||||
prediction.willCollide
|
||||
true // hasCollision
|
||||
);
|
||||
|
||||
// 如果存在风险，添加到结果中
|
||||
if (level != RiskLevel::NONE) {
|
||||
// 计算相对运动
|
||||
MovementVector v1v(vehicle1.speed, vehicle1.heading);
|
||||
MovementVector v2v(vehicle2.speed, vehicle2.heading);
|
||||
double vx = v1v.vx - v2v.vx;
|
||||
double vy = v1v.vy - v2v.vy;
|
||||
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
|
||||
|
||||
risks.push_back({
|
||||
vehicle1.vehicleNo,
|
||||
vehicle2.vehicleNo,
|
||||
level,
|
||||
currentDistance,
|
||||
prediction.minDistance,
|
||||
currentDistance, // 最小距离就是当前距离
|
||||
relativeSpeed,
|
||||
{vx, vy}, // 相对运动向量
|
||||
zoneType
|
||||
});
|
||||
|
||||
Logger::debug("检测到车辆间碰撞风险: vehicle1=", vehicle1.vehicleNo,
|
||||
"vehicle2=", vehicle2.vehicleNo,
|
||||
"distance=", currentDistance, "m",
|
||||
"level=", static_cast<int>(level),
|
||||
"zone=", static_cast<int>(zoneType),
|
||||
"相对速度=", relativeSpeed, "m/s",
|
||||
"相对运动=(", vx, ",", vy, ")",
|
||||
"正在远离=", relativeSpeed > 0 ? "是" : "否");
|
||||
", vehicle2=", vehicle2.vehicleNo,
|
||||
", distance=", currentDistance, "m, level=", static_cast<int>(level),
|
||||
", zone=", static_cast<int>(zoneType),
|
||||
", relativeSpeed=", relativeSpeed, "m/s, relativeMotion=(", vx, ",", vy, ")",
|
||||
", isMovingAway=", relativeSpeed > 0 ? "是" : "否");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 记录检测结果
|
||||
if (!risks.empty()) {
|
||||
Logger::debug("碰撞检测完成: 发现 ", risks.size(), "个风险");
|
||||
for (const auto& risk : risks) {
|
||||
Logger::debug("风险详情: id1=", risk.id1,
|
||||
", id2=", risk.id2,
|
||||
", distance=", risk.distance, "m, level=", static_cast<int>(risk.level),
|
||||
", zone=", static_cast<int>(risk.zoneType),
|
||||
", 相对速度=", risk.relativeSpeed,
|
||||
"m/s, 相对运动=(", risk.relativeMotion.x, ",", risk.relativeMotion.y, ")");
|
||||
}
|
||||
}
|
||||
|
||||
return risks;
|
||||
}
|
||||
|
||||
@ -283,343 +242,50 @@ std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
|
||||
return {level, zoneType};
|
||||
}
|
||||
|
||||
bool CollisionDetector::checkCollisionRisk(
|
||||
const Vector2D& pos1, double speed1, double heading1, const std::string& id1,
|
||||
const Vector2D& pos2, double speed2, double heading2, const std::string& id2,
|
||||
bool isAircraft1, bool isAircraft2) const {
|
||||
|
||||
// 获取区域配置
|
||||
const auto& areaConfig = getCollisionParams(pos1);
|
||||
const auto& predictionConfig = SystemConfig::instance().collision_detection.prediction;
|
||||
|
||||
// 计算当前距离
|
||||
double dx = pos1.x - pos2.x;
|
||||
double dy = pos1.y - pos2.y;
|
||||
double currentDistance = std::sqrt(dx*dx + dy*dy);
|
||||
|
||||
// 计算预测阈值
|
||||
double maxSpeed = std::max(speed1, speed2);
|
||||
double predictionDistance = maxSpeed * predictionConfig.time_window;
|
||||
// 根据物体类型选择基准阈值
|
||||
double baseThreshold = (isAircraft1 || isAircraft2) ?
|
||||
areaConfig.aircraftCollisionRadius :
|
||||
areaConfig.vehicleCollisionRadius;
|
||||
double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0);
|
||||
|
||||
// 如果当前距离超过预测阈值，直接返回false
|
||||
if (currentDistance > predictionThreshold) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 直接告警条件：距离小于阈值的一半
|
||||
if (currentDistance < baseThreshold * 0.5) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// 获取预测结果
|
||||
auto prediction = predictTrajectoryCollision(
|
||||
pos1, speed1, heading1,
|
||||
pos2, speed2, heading2,
|
||||
isAircraft1 ? predictionConfig.aircraft_size : predictionConfig.vehicle_size,
|
||||
isAircraft2 ? predictionConfig.aircraft_size : predictionConfig.vehicle_size,
|
||||
predictionConfig.time_window
|
||||
);
|
||||
|
||||
// 获取阈值配置
|
||||
auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2);
|
||||
double alertDistance = thresholds.critical;
|
||||
|
||||
// 如果距离在阈值范围内，进行相对运动分析
|
||||
bool hasCollisionRisk = false;
|
||||
if (currentDistance < baseThreshold) {
|
||||
// 使用 MovementVector 计算速度分量
|
||||
MovementVector mv1(speed1, heading1);
|
||||
MovementVector mv2(speed2, heading2);
|
||||
|
||||
// 计算相对速度
|
||||
double vx = mv1.vx - mv2.vx;
|
||||
double vy = mv1.vy - mv2.vy;
|
||||
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
|
||||
|
||||
// 计算相对运动（点积），判断是否在接近
|
||||
double relativeMotion = dx*vx + dy*vy;
|
||||
|
||||
// 如果相对运动小于等于0，且相对速度大于阈值，或者预测会碰撞，或者已经进入警戒区域，则有碰撞风险
|
||||
hasCollisionRisk = (relativeMotion <= 0 && relativeSpeed > 1.0) ||
|
||||
prediction.willCollide ||
|
||||
currentDistance <= alertDistance;
|
||||
|
||||
Logger::debug(
|
||||
"碰撞风险分析: id1=", id1,
|
||||
", id2=", id2,
|
||||
", 当前距离=", currentDistance,
|
||||
"m, 相对速度=", relativeSpeed,
|
||||
"m/s, 相对运动=", relativeMotion,
|
||||
", 正在远离=", prediction.isMovingAway ? "是" : "否"
|
||||
);
|
||||
}
|
||||
|
||||
if (hasCollisionRisk) {
|
||||
// 使用 MovementVector 计算相对运动，用于日志记录
|
||||
MovementVector mv1(speed1, heading1);
|
||||
MovementVector mv2(speed2, heading2);
|
||||
double vx = mv1.vx - mv2.vx;
|
||||
double vy = mv1.vy - mv2.vy;
|
||||
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
|
||||
|
||||
std::string type1 = isAircraft1 ? "航空器" : "车辆";
|
||||
std::string type2 = isAircraft2 ? "航空器" : "车辆";
|
||||
|
||||
Logger::debug(
|
||||
"检测到碰撞风险:",
|
||||
"type1=", type1,
|
||||
"id1=", id1,
|
||||
"type2=", type2,
|
||||
"id2=", id2,
|
||||
"distance=", currentDistance, "m,",
|
||||
"relativeSpeed=", relativeSpeed, "m/s,",
|
||||
"prediction=", prediction.willCollide ? "是" : "否"
|
||||
);
|
||||
}
|
||||
|
||||
return hasCollisionRisk;
|
||||
}
|
||||
|
||||
bool CollisionDetector::checkAircraftVehicleCollision(const Aircraft& aircraft,
|
||||
const Vehicle& vehicle) const {
|
||||
return checkCollisionRisk(
|
||||
aircraft.position, aircraft.speed, aircraft.heading, aircraft.flightNo,
|
||||
vehicle.position, vehicle.speed, vehicle.heading, vehicle.vehicleNo,
|
||||
true, false
|
||||
);
|
||||
}
|
||||
|
||||
bool CollisionDetector::checkVehicleCollision(const Vehicle& v1, const Vehicle& v2) const {
|
||||
return checkCollisionRisk(
|
||||
v1.position, v1.speed, v1.heading, v1.vehicleNo,
|
||||
v2.position, v2.speed, v2.heading, v2.vehicleNo,
|
||||
false, false
|
||||
);
|
||||
}
|
||||
|
||||
CollisionPrediction CollisionDetector::predictTrajectoryCollision(
|
||||
const Vector2D& pos1, double speed1, double heading1,
|
||||
const Vector2D& pos2, double speed2, double heading2,
|
||||
double size1, double size2, double timeWindow) const {
|
||||
|
||||
CollisionPrediction result;
|
||||
result.willCollide = false;
|
||||
result.timeToCollision = std::numeric_limits<double>::infinity();
|
||||
result.minDistance = std::numeric_limits<double>::infinity();
|
||||
|
||||
// 计算安全距离等于两个物体尺寸之和，加上 20m 的缓冲区
|
||||
double safeDistance = size1 + size2 + 20.0;
|
||||
|
||||
// 计算速度分量
|
||||
MovementVector mv1(speed1, heading1);
|
||||
MovementVector mv2(speed2, heading2);
|
||||
|
||||
// 计算相对速度
|
||||
double dvx = mv1.vx - mv2.vx;
|
||||
double dvy = mv1.vy - mv2.vy;
|
||||
double relativeSpeed = std::sqrt(dvx*dvx + dvy*dvy);
|
||||
|
||||
// 计算相对位置
|
||||
double dx = pos1.x - pos2.x;
|
||||
double dy = pos1.y - pos2.y;
|
||||
double currentDistance = std::sqrt(dx*dx + dy*dy);
|
||||
|
||||
// 计算相对运动方向（点积）
|
||||
double relativeMotion = dx*dvx + dy*dvy;
|
||||
result.isMovingAway = (relativeMotion > 0); // 添加到结果中
|
||||
|
||||
// 记录初始状态
|
||||
Logger::debug(
|
||||
"轨迹预测初始状态: 当前距离=", currentDistance,
|
||||
"m, 相对速度=", relativeSpeed,
|
||||
"m/s, 安全距离=", safeDistance,
|
||||
"m, 相对运动=", relativeMotion,
|
||||
", 正在远离=", result.isMovingAway ? "是" : "否"
|
||||
);
|
||||
|
||||
// 如果物体相对静止，直接返回当前状态
|
||||
if (relativeSpeed < 1e-6) {
|
||||
result.minDistance = currentDistance;
|
||||
result.willCollide = (currentDistance <= safeDistance);
|
||||
if (result.willCollide) {
|
||||
result.timeToCollision = 0.0;
|
||||
// 根据物体尺寸计算碰撞点
|
||||
double ratio = size1 / (size1 + size2);
|
||||
result.collisionPoint.x = pos2.x + dx * ratio;
|
||||
result.collisionPoint.y = pos2.y + dy * ratio;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// 如果当前距离小于安全距离，且相对运动为负（正在接近），直接判定为碰撞
|
||||
if (currentDistance <= safeDistance && relativeMotion < 0) {
|
||||
result.willCollide = true;
|
||||
result.timeToCollision = 0.0;
|
||||
result.minDistance = currentDistance;
|
||||
result.collisionPoint = {
|
||||
pos2.x + dx * (size1 / (size1 + size2)),
|
||||
pos2.y + dy * (size1 / (size1 + size2))
|
||||
};
|
||||
|
||||
Logger::debug(
|
||||
"预测到碰撞: 当前距离=", currentDistance,
|
||||
"m < 安全距离=", safeDistance,
|
||||
"m, 且正在接近, 相对运动=", relativeMotion
|
||||
);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 计算相对运动方程的系数
|
||||
double a = dvx * dvx + dvy * dvy;
|
||||
double b = 2.0 * (dx * dvx + dy * dvy);
|
||||
double c = dx * dx + dy * dy - safeDistance * safeDistance;
|
||||
|
||||
// 计算判别式
|
||||
double discriminant = b * b - 4.0 * a * c;
|
||||
|
||||
// 如果判别式小于0，不会发生碰撞
|
||||
if (discriminant < 0) {
|
||||
// 计算时间窗口内的最小距离
|
||||
double t_min = -b / (2.0 * a);
|
||||
t_min = std::clamp(t_min, 0.0, timeWindow);
|
||||
|
||||
// 计算最小距离时刻的位置
|
||||
double x1_min = pos1.x + mv1.vx * t_min;
|
||||
double y1_min = pos1.y + mv1.vy * t_min;
|
||||
double x2_min = pos2.x + mv2.vx * t_min;
|
||||
double y2_min = pos2.y + mv2.vy * t_min;
|
||||
|
||||
result.minDistance = std::sqrt(
|
||||
std::pow(x1_min - x2_min, 2) +
|
||||
std::pow(y1_min - y2_min, 2)
|
||||
);
|
||||
|
||||
Logger::debug(
|
||||
"无碰撞可能: 最小距离=", result.minDistance,
|
||||
"m, 发生时间=", t_min, "s"
|
||||
);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 计算可能的碰撞时间点
|
||||
double t1 = (-b - std::sqrt(discriminant)) / (2.0 * a);
|
||||
double t2 = (-b + std::sqrt(discriminant)) / (2.0 * a);
|
||||
|
||||
// 检查两个时间点
|
||||
for (double t : {t1, t2}) {
|
||||
if (t >= 0 && t <= timeWindow) {
|
||||
// 计算t时刻的位置
|
||||
double x1_t = pos1.x + mv1.vx * t;
|
||||
double y1_t = pos1.y + mv1.vy * t;
|
||||
double x2_t = pos2.x + mv2.vx * t;
|
||||
double y2_t = pos2.y + mv2.vy * t;
|
||||
|
||||
// 计算t时刻的距离
|
||||
double dist_t = std::sqrt(
|
||||
std::pow(x1_t - x2_t, 2) +
|
||||
std::pow(y1_t - y2_t, 2)
|
||||
);
|
||||
|
||||
// 更新最小距离
|
||||
if (dist_t < result.minDistance) {
|
||||
result.minDistance = dist_t;
|
||||
|
||||
// 如果距离小于等于安全距离，记录碰撞信息
|
||||
if (dist_t <= safeDistance && !result.willCollide) {
|
||||
result.willCollide = true;
|
||||
result.timeToCollision = t;
|
||||
|
||||
// 根据物体尺寸计算碰撞点
|
||||
double ratio = size1 / (size1 + size2);
|
||||
result.collisionPoint.x = x2_t + (x1_t - x2_t) * ratio;
|
||||
result.collisionPoint.y = y2_t + (y1_t - y2_t) * ratio;
|
||||
|
||||
Logger::debug(
|
||||
"预测到碰撞: 时间=", t,
|
||||
"s, 距离=", dist_t,
|
||||
"m, 位置=(", result.collisionPoint.x,
|
||||
",", result.collisionPoint.y, ")",
|
||||
", 正在远离=", result.isMovingAway ? "是" : "否"
|
||||
);
|
||||
}
|
||||
else {
|
||||
Logger::debug("没有预测到碰撞: 时间=", t, "s, 距离=", dist_t, "m, 位置=(", x2_t, ",", y2_t, ")", ", 正在远离=", result.isMovingAway ? "是" : "否");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 基于矩形的航空器和车辆碰撞检测
|
||||
bool CollisionDetector::checkRectangleBasedAircraftVehicleCollision(
|
||||
const Aircraft& aircraft,
|
||||
const Aircraft& aircraft,
|
||||
const Vehicle& vehicle,
|
||||
double timeWindow) const {
|
||||
|
||||
// 获取系统配置
|
||||
const auto& config = SystemConfig::instance();
|
||||
// 创建矩形
|
||||
collision::Rectangle rect1{
|
||||
aircraft.position,
|
||||
SystemConfig::instance().collision_detection.prediction.aircraft_size,
|
||||
{aircraft.speed * std::sin(aircraft.heading * M_PI / 180.0),
|
||||
aircraft.speed * std::cos(aircraft.heading * M_PI / 180.0)}
|
||||
};
|
||||
|
||||
// 计算速度分量
|
||||
MovementVector mv1(aircraft.speed, aircraft.heading);
|
||||
MovementVector mv2(vehicle.speed, vehicle.heading);
|
||||
|
||||
// 创建航空器和车辆的正方形表示
|
||||
collision::Rectangle aircraftRect(
|
||||
aircraft.position, // 中心点
|
||||
config.collision_detection.prediction.aircraft_size * 2.0, // 边长
|
||||
Vector2D{mv1.vx, mv1.vy}, // 速度向量
|
||||
aircraft.heading // 朝向
|
||||
);
|
||||
|
||||
collision::Rectangle vehicleRect(
|
||||
vehicle.position, // 中心点
|
||||
config.collision_detection.prediction.vehicle_size * 2.0, // 边长
|
||||
Vector2D{mv2.vx, mv2.vy}, // 速度向量
|
||||
vehicle.heading // 朝向
|
||||
);
|
||||
collision::Rectangle rect2{
|
||||
vehicle.position,
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size,
|
||||
{vehicle.speed * std::sin(vehicle.heading * M_PI / 180.0),
|
||||
vehicle.speed * std::cos(vehicle.heading * M_PI / 180.0)}
|
||||
};
|
||||
|
||||
// 检测碰撞
|
||||
return aircraftRect.checkCollision(vehicleRect, timeWindow);
|
||||
return collision::predictRectangleBasedCollision(rect1, rect2, timeWindow);
|
||||
}
|
||||
|
||||
// 基于矩形的车辆间碰撞检测
|
||||
bool CollisionDetector::checkRectangleBasedVehicleCollision(
|
||||
const Vehicle& v1,
|
||||
const Vehicle& v1,
|
||||
const Vehicle& v2,
|
||||
double timeWindow) const {
|
||||
|
||||
// 获取系统配置
|
||||
const auto& config = SystemConfig::instance();
|
||||
double vehicleSize = config.collision_detection.prediction.vehicle_size;
|
||||
// 创建矩形
|
||||
collision::Rectangle rect1{
|
||||
v1.position,
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size,
|
||||
{v1.speed * std::sin(v1.heading * M_PI / 180.0),
|
||||
v1.speed * std::cos(v1.heading * M_PI / 180.0)}
|
||||
};
|
||||
|
||||
// 计算速度分量
|
||||
MovementVector mv1(v1.speed, v1.heading);
|
||||
MovementVector mv2(v2.speed, v2.heading);
|
||||
|
||||
// 创建两个车辆的正方形表示
|
||||
collision::Rectangle rect1(
|
||||
v1.position, // 中心点
|
||||
vehicleSize * 2.0, // 边长
|
||||
Vector2D{mv1.vx, mv1.vy}, // 速度向量
|
||||
v1.heading // 朝向
|
||||
);
|
||||
|
||||
collision::Rectangle rect2(
|
||||
v2.position, // 中心点
|
||||
vehicleSize * 2.0, // 边长
|
||||
Vector2D{mv2.vx, mv2.vy}, // 速度向量
|
||||
v2.heading // 朝向
|
||||
);
|
||||
collision::Rectangle rect2{
|
||||
v2.position,
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size,
|
||||
{v2.speed * std::sin(v2.heading * M_PI / 180.0),
|
||||
v2.speed * std::cos(v2.heading * M_PI / 180.0)}
|
||||
};
|
||||
|
||||
// 检测碰撞
|
||||
return rect1.checkCollision(rect2, timeWindow);
|
||||
return collision::predictRectangleBasedCollision(rect1, rect2, timeWindow);
|
||||
}
|
||||
@ -35,15 +35,6 @@ struct CollisionRisk {
|
||||
WarningZoneType zoneType; // 预警区域类型
|
||||
};
|
||||
|
||||
// 轨迹预测碰撞结果
|
||||
struct CollisionPrediction {
|
||||
bool willCollide; // 是否可能发生碰撞
|
||||
double timeToCollision; // 到碰撞点的时间(秒)
|
||||
Vector2D collisionPoint; // 可能的碰撞点
|
||||
double minDistance; // 最小距离
|
||||
bool isMovingAway; // 是否正在远离
|
||||
};
|
||||
|
||||
class CollisionDetector {
|
||||
public:
|
||||
CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles);
|
||||
@ -69,15 +60,6 @@ public:
|
||||
double timeWindow = 30.0
|
||||
) const;
|
||||
|
||||
// 基于轨迹预测的碰撞检测
|
||||
CollisionPrediction predictTrajectoryCollision(
|
||||
const Vector2D& pos1, double speed1, double heading1,
|
||||
const Vector2D& pos2, double speed2, double heading2,
|
||||
double size1,
|
||||
double size2,
|
||||
double timeWindow = 30.0
|
||||
) const;
|
||||
|
||||
private:
|
||||
const AirportBounds& airportBounds_;
|
||||
QuadTree<Vehicle> vehicleTree_;
|
||||
@ -90,24 +72,6 @@ private:
|
||||
return airportBounds_.getAreaConfig(areaType);
|
||||
}
|
||||
|
||||
// 通用碰撞风险检测函数
|
||||
bool checkCollisionRisk(
|
||||
const Vector2D& pos1, double speed1, double heading1, const std::string& id1,
|
||||
const Vector2D& pos2, double speed2, double heading2, const std::string& id2,
|
||||
bool isAircraft1, bool isAircraft2) const;
|
||||
|
||||
// 检查航空器和车辆是否可能碰撞
|
||||
bool checkAircraftVehicleCollision(const Aircraft& aircraft, const Vehicle& vehicle) const;
|
||||
// 检查两辆车是否可能碰撞
|
||||
bool checkVehicleCollision(const Vehicle& v1, const Vehicle& v2) const;
|
||||
|
||||
// 计算风险等级
|
||||
RiskLevel calculateRiskLevel(double distance, const Vector2D& position,
|
||||
bool isAircraft1, bool isAircraft2) const;
|
||||
|
||||
// 确定预警区域类型
|
||||
WarningZoneType determineWarningZone(double distance, double threshold) const;
|
||||
|
||||
// 统一的风险评估函数
|
||||
std::pair<RiskLevel, WarningZoneType> evaluateRisk(
|
||||
double currentDistance,
|
||||
@ -133,26 +97,6 @@ private:
|
||||
"°, vx=", vx, "m/s, vy=", vy, "m/s");
|
||||
}
|
||||
};
|
||||
|
||||
// 创建正方形
|
||||
collision::Rectangle createRectangle(
|
||||
const Vector2D& position,
|
||||
double size,
|
||||
double heading,
|
||||
double speed
|
||||
) const {
|
||||
// 计算速度分量
|
||||
MovementVector mv(speed, heading);
|
||||
Logger::debug("速度分量计算: speed=" + std::to_string(speed) + "m/s, heading=" +
|
||||
std::to_string(heading) + "°, vx=" + std::to_string(mv.vx) +
|
||||
"m/s, vy=" + std::to_string(mv.vy) + "m/s");
|
||||
|
||||
return collision::Rectangle{
|
||||
position, // 中心点
|
||||
size, // 正方形边长
|
||||
{mv.vx, mv.vy} // 速度向量
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
#endif // AIRPORT_DETECTOR_COLLISION_DETECTOR_H
|
||||
@ -26,12 +26,6 @@ struct Rectangle {
|
||||
// 构造函数
|
||||
Rectangle(const Vector2D& c, double s, const Vector2D& v, double h = 0.0)
|
||||
: center(c), size(s), velocity(v), heading(h) {
|
||||
#ifdef TESTING
|
||||
std::cout << "正方形构造: 中心点=(" << center.x << "," <<
|
||||
center.y << "), 边长=" << size <<
|
||||
", 速度=(" << velocity.x << "," <<
|
||||
velocity.y << ")" << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
// 默认构造函数
|
||||
@ -54,87 +48,73 @@ struct Rectangle {
|
||||
future.center.y += velocity.y * t;
|
||||
return future;
|
||||
}
|
||||
};
|
||||
|
||||
// 检测与另一个正方形是否碰撞
|
||||
bool checkCollision(const Rectangle& other, double timeWindow = 1.0) const {
|
||||
// 1. 首先检查当前是否碰撞
|
||||
double x1_min, x1_max, y1_min, y1_max;
|
||||
double x2_min, x2_max, y2_min, y2_max;
|
||||
// 预测两个矩形在给定时间窗口内是否会发生碰撞
|
||||
inline bool predictRectangleBasedCollision(const Rectangle& rect1, const Rectangle& rect2, double timeWindow) {
|
||||
// 1. 首先检查当前是否碰撞
|
||||
double x1_min, x1_max, y1_min, y1_max;
|
||||
double x2_min, x2_max, y2_min, y2_max;
|
||||
|
||||
rect1.getAABB(x1_min, x1_max, y1_min, y1_max);
|
||||
rect2.getAABB(x2_min, x2_max, y2_min, y2_max);
|
||||
|
||||
// 检查当前是否重叠
|
||||
if (x1_max >= x2_min && x2_max >= x1_min &&
|
||||
y1_max >= y2_min && y2_max >= y1_min) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// 2. 检查未来一段时间内是否会碰撞
|
||||
// 计算相对速度
|
||||
double rel_vx = rect1.velocity.x - rect2.velocity.x;
|
||||
double rel_vy = rect1.velocity.y - rect2.velocity.y;
|
||||
double rel_speed = std::sqrt(rel_vx*rel_vx + rel_vy*rel_vy);
|
||||
|
||||
// 如果相对速度接近0，使用当前距离判断
|
||||
if (rel_speed < 0.1) {
|
||||
double dx = rect1.center.x - rect2.center.x;
|
||||
double dy = rect1.center.y - rect2.center.y;
|
||||
double distance = std::sqrt(dx*dx + dy*dy);
|
||||
double safe_distance = rect1.size/2.0 + rect2.size/2.0; // 两个正方形从中心到边缘的距离之和
|
||||
|
||||
this->getAABB(x1_min, x1_max, y1_min, y1_max);
|
||||
other.getAABB(x2_min, x2_max, y2_min, y2_max);
|
||||
if (distance <= safe_distance) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3. 在时间窗口内采样检查
|
||||
const int STEPS = 60;
|
||||
double dt = timeWindow / STEPS;
|
||||
|
||||
for (int i = 1; i <= STEPS; ++i) {
|
||||
double t = i * dt;
|
||||
Rectangle future1 = rect1.predictPosition(t);
|
||||
Rectangle future2 = rect2.predictPosition(t);
|
||||
|
||||
// 检查当前是否重叠
|
||||
future1.getAABB(x1_min, x1_max, y1_min, y1_max);
|
||||
future2.getAABB(x2_min, x2_max, y2_min, y2_max);
|
||||
|
||||
// 检查是否重叠
|
||||
if (x1_max >= x2_min && x2_max >= x1_min &&
|
||||
y1_max >= y2_min && y2_max >= y1_min) {
|
||||
#ifdef TESTING
|
||||
std::cout << "检测到当<EFBFBD><EFBFBD><EFBFBD>碰撞" << std::endl;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
// 2. 检查未来一段时间内是否会碰撞
|
||||
// 计算相对速度
|
||||
double rel_vx = velocity.x - other.velocity.x;
|
||||
double rel_vy = velocity.y - other.velocity.y;
|
||||
double rel_speed = std::sqrt(rel_vx*rel_vx + rel_vy*rel_vy);
|
||||
// 计算中心点距离
|
||||
double dx = (future1.center.x - future2.center.x);
|
||||
double dy = (future1.center.y - future2.center.y);
|
||||
double distance = std::sqrt(dx*dx + dy*dy);
|
||||
double safe_distance = rect1.size/2.0 + rect2.size/2.0; // 两个正方形从中心到边缘的距离之和
|
||||
|
||||
// 如果相对速度接近0，使用当前距离判断
|
||||
if (rel_speed < 0.1) {
|
||||
double dx = center.x - other.center.x;
|
||||
double dy = center.y - other.center.y;
|
||||
double distance = std::sqrt(dx*dx + dy*dy);
|
||||
double safe_distance = size/2.0 + other.size/2.0; // 两个正方形从中心到边缘的距离之和
|
||||
|
||||
if (distance <= safe_distance) {
|
||||
#ifdef TESTING
|
||||
std::cout << "静止状态检测到碰撞，距离=" << distance << "米" << std::endl;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
if (distance <= safe_distance) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// 3. 在时间窗口内采样检查
|
||||
const int STEPS = 20;
|
||||
double dt = timeWindow / STEPS;
|
||||
|
||||
for (int i = 1; i <= STEPS; ++i) {
|
||||
double t = i * dt;
|
||||
Rectangle future1 = predictPosition(t);
|
||||
Rectangle future2 = other.predictPosition(t);
|
||||
|
||||
future1.getAABB(x1_min, x1_max, y1_min, y1_max);
|
||||
future2.getAABB(x2_min, x2_max, y2_min, y2_max);
|
||||
|
||||
// 检查是否重叠
|
||||
if (x1_max >= x2_min && x2_max >= x1_min &&
|
||||
y1_max >= y2_min && y2_max >= y1_min) {
|
||||
#ifdef TESTING
|
||||
std::cout << "预测到碰撞，时间=" << t << "秒" << std::endl;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
// 计算中心点距离
|
||||
double dx = (future1.center.x - future2.center.x);
|
||||
double dy = (future1.center.y - future2.center.y);
|
||||
double distance = std::sqrt(dx*dx + dy*dy);
|
||||
double safe_distance = size/2.0 + other.size/2.0; // 两个正方形从中心到边缘的距离之和
|
||||
|
||||
if (distance <= safe_distance) {
|
||||
#ifdef TESTING
|
||||
std::cout << "预测到危险接近，时间=" << t
|
||||
<< "秒，距离=" << distance
|
||||
<< "米，安全距离=" << safe_distance << "米" << std::endl;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace collision
|
||||
|
||||
|
||||
@ -103,7 +103,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) {
|
||||
|
||||
Vehicle vehicle;
|
||||
vehicle.vehicleNo = "VEH001";
|
||||
vehicle.position = {120, 100}; // 离航空器20米
|
||||
vehicle.position = {120, 100}; // <EFBFBD><EFBFBD><EFBFBD>离航空器20米
|
||||
vehicle.speed = 5;
|
||||
vehicle.heading = 270;
|
||||
Logger::info("Created vehicle: vehicleNo=", vehicle.vehicleNo,
|
||||
@ -136,7 +136,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleOtherVehicleCollision) {
|
||||
EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH002"))
|
||||
.WillRepeatedly(testing::Return(false));
|
||||
|
||||
// 设置测试数据
|
||||
// 设置测试数<EFBFBD><EFBFBD><EFBFBD>
|
||||
Vehicle controlVehicle;
|
||||
controlVehicle.vehicleNo = "VEH001";
|
||||
controlVehicle.position = {100, 100};
|
||||
@ -254,7 +254,7 @@ TEST_F(CollisionDetectorTest, PerformanceTest) {
|
||||
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
|
||||
.WillRepeatedly(testing::Return(false));
|
||||
|
||||
// 设置3辆可控车辆
|
||||
// 设3辆可控车辆
|
||||
std::vector<std::string> controlVehicleNos = {
|
||||
"VEH001", // 滑行道上的可控车辆
|
||||
"VEH002", // 停机位的可控车辆
|
||||
@ -291,7 +291,7 @@ TEST_F(CollisionDetectorTest, PerformanceTest) {
|
||||
aircraft.push_back(a);
|
||||
}
|
||||
|
||||
// 停机位区域：100架航空器，180辆车辆
|
||||
// 停机位区域：100架航空器，180辆<EFBFBD><EFBFBD><EFBFBD>辆
|
||||
for (int i = 0; i < 100; ++i) {
|
||||
Aircraft a;
|
||||
a.flightNo = "GT" + std::to_string(i + 1);
|
||||
@ -391,7 +391,8 @@ TEST_F(CollisionDetectorTest, PerformanceTest) {
|
||||
// 修改矩形碰撞检测测试
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionParallelMotion) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 10.0; // 改为10米全边长
|
||||
SystemConfig::instance().collision_detection.prediction.aircraft_size = 20.0; // 改为20米全边长
|
||||
|
||||
// 创建两个平行运动的车辆
|
||||
Vehicle v1;
|
||||
@ -407,15 +408,16 @@ TEST_F(CollisionDetectorTest, RectangleCollisionParallelMotion) {
|
||||
v2.heading = 90.0; // 向东行驶
|
||||
|
||||
// 检测碰撞
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
|
||||
|
||||
// 由于两车间距小于安全距离，应该检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "平行运动的车辆应该检测到碰撞";
|
||||
// 由<EFBFBD><EFBFBD><EFBFBD>两车间距小于安全距离（10米），应该在30秒时间窗口内检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "平行运动的车辆应该在30秒时间窗口内检测到碰撞";
|
||||
}
|
||||
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionCrossingPaths) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0;
|
||||
|
||||
// 创建两个交叉路径的车辆
|
||||
Vehicle v1;
|
||||
@ -431,16 +433,16 @@ TEST_F(CollisionDetectorTest, RectangleCollisionCrossingPaths) {
|
||||
v2.heading = 180.0; // 向南行驶
|
||||
|
||||
// 检测碰撞
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
|
||||
|
||||
// 两车将在(100, 100)处相遇，应该检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "交叉路径的车辆应该检测到碰撞";
|
||||
// 两车将在(100, 100)处相遇，应该在30秒时间窗口内检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "交叉路径的车辆应该在30秒时间窗口内检测到碰撞";
|
||||
}
|
||||
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0;
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 10.0; // 改为10米全边长
|
||||
SystemConfig::instance().collision_detection.prediction.aircraft_size = 20.0; // 改为20米全边长
|
||||
|
||||
// 创建一个航空器和一个车辆
|
||||
Aircraft aircraft;
|
||||
@ -455,6 +457,7 @@ TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) {
|
||||
vehicle.speed = 8.0;
|
||||
vehicle.heading = 270.0; // 向西行驶（与航空器相向而行）
|
||||
|
||||
// 输出测试配置
|
||||
std::cout << "\n=== 矩形碰撞检测测试（航空器-车辆） ===" << std::endl;
|
||||
std::cout << "配置信息：" << std::endl;
|
||||
std::cout << "- 航空器尺寸: " << SystemConfig::instance().collision_detection.prediction.aircraft_size << "米" << std::endl;
|
||||
@ -487,23 +490,31 @@ TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) {
|
||||
std::cout << "预计碰撞时间: " << timeToCollision << "秒" << std::endl;
|
||||
|
||||
// 检测碰撞
|
||||
bool hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
|
||||
bool hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0);
|
||||
std::cout << "\n碰撞检测结果: " << (hasCollision ? "是" : "否") << std::endl;
|
||||
|
||||
// 由于距离较近且相向而行，应该检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "相向而行且距离较近的航空器和车辆应该检测到碰撞";
|
||||
// 由于距离较近且相向而行，应该在30秒时间窗口内检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "相向而行且距离较近的航空器和车辆应该在30秒时间窗口内检测到碰撞";
|
||||
|
||||
// 测试边界情况：增加距离
|
||||
vehicle.position.x = 150.0; // 增加到50米距离
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
|
||||
EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞";
|
||||
vehicle.position = {900.0, 100.0}; // 增加到800米距离
|
||||
dx = vehicle.position.x - aircraft.position.x;
|
||||
dy = vehicle.position.y - aircraft.position.y;
|
||||
distance = std::sqrt(dx*dx + dy*dy);
|
||||
std::cout << "\n增加距离后：" << std::endl;
|
||||
std::cout << "- 车辆新位置: (" << vehicle.position.x << ", " << vehicle.position.y << ")" << std::endl;
|
||||
std::cout << "- 实际距离: " << distance << "米" << std::endl;
|
||||
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0);
|
||||
std::cout << "- 碰撞检测结果: " << (hasCollision ? "是" : "否") << std::endl;
|
||||
EXPECT_FALSE(hasCollision) << "距离较远时不应该在30秒时间窗口内检测到碰撞";
|
||||
|
||||
// 测试边界情况：静止状态
|
||||
vehicle.position.x = 110.0; // 距离缩短到10米
|
||||
vehicle.position = {110.0, 100.0}; // 距离缩短到10米
|
||||
vehicle.speed = 0.0;
|
||||
aircraft.speed = 0.0;
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
|
||||
EXPECT_TRUE(hasCollision) << "静止状态下距离较近时应该检测到碰撞";
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0);
|
||||
EXPECT_TRUE(hasCollision) << "静止状态下距离较近时应该在30秒时间窗口内检测到碰撞";
|
||||
|
||||
// 测试边界情况：垂直路径
|
||||
vehicle.position = {100.0, 120.0}; // 在航空器正北方
|
||||
@ -511,13 +522,14 @@ TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) {
|
||||
vehicle.heading = 180.0; // 向南行驶
|
||||
aircraft.speed = 10.0;
|
||||
aircraft.heading = 90.0; // 向东行驶
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
|
||||
EXPECT_TRUE(hasCollision) << "垂直路径且距离较近时应该检测到碰撞";
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0);
|
||||
EXPECT_TRUE(hasCollision) << "垂直路径且距离较近时应该在30秒时间窗口内检测到碰撞";
|
||||
}
|
||||
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionWithRotation) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0;
|
||||
|
||||
// 创建两个有一定角度的车辆
|
||||
Vehicle v1;
|
||||
@ -533,22 +545,23 @@ TEST_F(CollisionDetectorTest, RectangleCollisionWithRotation) {
|
||||
v2.heading = 315.0; // 向西北行驶
|
||||
|
||||
// 检测碰撞
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
|
||||
|
||||
// 由于两车轨迹交叉且距离较近，应该检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "不同角度的车辆相交时应该检测到碰撞";
|
||||
// 由于两车轨迹交叉且距离较近，应该在30秒时间窗口内检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "不同角度的车辆相交时应该在30秒时间窗口内检测到碰撞";
|
||||
|
||||
// 将v2移动到更远的位置
|
||||
v2.position = {150.0, 150.0};
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
|
||||
|
||||
// 距离较远时不应该检测到碰撞
|
||||
EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞";
|
||||
EXPECT_FALSE(hasCollision) << "距离较远时不应该在30秒时间窗口内检测到碰撞";
|
||||
}
|
||||
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionEdgeCases) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0;
|
||||
|
||||
Vehicle v1;
|
||||
v1.vehicleNo = "VEH001";
|
||||
@ -560,21 +573,21 @@ TEST_F(CollisionDetectorTest, RectangleCollisionEdgeCases) {
|
||||
v2.vehicleNo = "VEH002";
|
||||
|
||||
// 测试1：两个静止车辆，刚好接触
|
||||
v2.position = {110.0, 100.0}; // 距离为10米（两车长度和）
|
||||
v2.position = {105.0, 100.0}; // 距离为5米（两车长度和的一半）
|
||||
v2.speed = 0.0;
|
||||
v2.heading = 90.0;
|
||||
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
EXPECT_TRUE(hasCollision) << "两个静止且刚好接触的车辆应该检测到碰撞";
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
|
||||
EXPECT_TRUE(hasCollision) << "两个静止且刚好接触的车辆应该在30秒时间窗口内检测到碰撞";
|
||||
|
||||
// 测试2：两个静止车辆，距离略大于安全距离
|
||||
v2.position = {111.0, 100.0}; // 距离为11米
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
EXPECT_FALSE(hasCollision) << "两个静止且距离大于安全距离的车辆不应该检测到碰撞";
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
|
||||
EXPECT_FALSE(hasCollision) << "两个静止且距离大于安全距离的车辆不应该在30秒时间窗口内检测到碰撞";
|
||||
|
||||
// 测试3：极低速度
|
||||
v2.position = {120.0, 100.0};
|
||||
v2.speed = 0.1; // 极低速度
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
EXPECT_FALSE(hasCollision) << "距离足够且速度极低的车辆不应该检测到碰撞";
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
|
||||
EXPECT_FALSE(hasCollision) << "距离足够且速度极低的车辆不应该在30秒时间窗口内检测到碰撞";
|
||||
}
|
||||
@ -667,7 +667,7 @@ def switch_traffic_light_state():
|
||||
lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9
|
||||
|
||||
old_state = traffic_light_data[1]["state"]
|
||||
traffic_light_data[1]["state"] = 1 if lat_diff > (AIRCRAFT_SIZE_M + DIST_50M) else 0
|
||||
traffic_light_data[1]["state"] = 1 if lat_diff > DIST_50M else 0
|
||||
|
||||
if old_state != traffic_light_data[1]["state"]:
|
||||
print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}")
|
||||
|
||||
Loading…
Reference in New Issue
Block a user