tian/CollisionAvoidance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改碰撞逻辑以圆形为基础

Browse Source
This commit is contained in:
Tian jianyong 2024-12-14 17:32:24 +08:00
parent 3c3b4190b8
commit 5ddd8a09a7
13 changed files with 1244 additions and 680 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
.cursorrules
View File

@ -1,3 +1,17 @@
请遵循：
- 最小化修改代码，不要修改任何与解决当前问题无关的代码（包括注释），一定不能删除任何其他的代码
- 使用 C++20 标准
- 使用 CMake 3.14 及以上版本
- 使用 nlohmann_json 3.11.3 版本
- 使用 FetchContent 管理第三方库
- 使用 Google Test 进行单元测试
- 使用 Google Mock 进行单元测试
- 使用 Google Benchmark 进行性能测试
- 在头文件中定义函数，在源文件中实现函数
- 在头文件中定义类，在源文件中实现类
- 使用源代码中的格式化方式和注释风格
- 日志的字符串格式采用拼接方式，不要使用{}占位符
请遵循以下 Markdown 规范：
1. 标题层级

1
CMakeLists.txt
View File

@ -95,6 +95,7 @@ if(EXISTS "${CMAKE_SOURCE_DIR}/tests")
# 测试源文件列表
set(TEST_SOURCES
tests/BasicCollisionTest.cpp
tests/CollisionDetectorTest.cpp
tests/BasicTypesTest.cpp
tests/HTTPDataSourceTest.cpp

95
config/airport_bounds.json
View File

@ -16,11 +16,22 @@
"height": 1
},
"config": {
"vehicle_collision_radius": 50.0,
"aircraft_collision_radius": 100.0,
"collision_radius": {
"aircraft": 100.0,
"special": 50.0,
"unmanned": 25.0
},
"height_threshold": 15.0,
"warning_zone_radius": 100.0,
"alert_zone_radius": 50.0
"warning_zone_radius": {
"aircraft": 200.0,
"special": 100.0,
"unmanned": 50.0
},
"alert_zone_radius": {
"aircraft": 100.0,
"special": 50.0,
"unmanned": 25.0
}
}
},
"taxiway": {
@ -31,11 +42,22 @@
"height": 1
},
"config": {
"vehicle_collision_radius": 30.0,
"aircraft_collision_radius": 50.0,
"collision_radius": {
"aircraft": 50.0,
"special": 50.0,
"unmanned": 25.0
},
"height_threshold": 10.0,
"warning_zone_radius": 50.0,
"alert_zone_radius": 25.0
"warning_zone_radius": {
"aircraft": 100.0,
"special": 100.0,
"unmanned": 50.0
},
"alert_zone_radius": {
"aircraft": 50.0,
"special": 50.0,
"unmanned": 25.0
}
}
},
"gate": {
@ -46,11 +68,22 @@
"height": 1
},
"config": {
"vehicle_collision_radius": 20.0,
"aircraft_collision_radius": 40.0,
"collision_radius": {
"aircraft": 40.0,
"special": 50.0,
"unmanned": 25.0
},
"height_threshold": 5.0,
"warning_zone_radius": 40.0,
"alert_zone_radius": 20.0
"warning_zone_radius": {
"aircraft": 80.0,
"special": 100.0,
"unmanned": 50.0
},
"alert_zone_radius": {
"aircraft": 40.0,
"special": 50.0,
"unmanned": 25.0
}
}
},
"service": {
@ -61,11 +94,22 @@
"height": 1
},
"config": {
"vehicle_collision_radius": 15.0,
"aircraft_collision_radius": 30.0,
"collision_radius": {
"aircraft": 30.0,
"special": 50.0,
"unmanned": 25.0
},
"height_threshold": 5.0,
"warning_zone_radius": 30.0,
"alert_zone_radius": 15.0
"warning_zone_radius": {
"aircraft": 60.0,
"special": 100.0,
"unmanned": 50.0
},
"alert_zone_radius": {
"aircraft": 30.0,
"special": 50.0,
"unmanned": 25.0
}
}
},
"test_zone": {
@ -76,11 +120,22 @@
"height": 4000
},
"config": {
"vehicle_collision_radius": 25.0,
"aircraft_collision_radius": 50.0,
"collision_radius": {
"aircraft": 50.0,
"special": 50.0,
"unmanned": 25.0
},
"height_threshold": 10.0,
"warning_zone_radius": 100.0,
"alert_zone_radius": 50.0
"warning_zone_radius": {
"aircraft": 100.0,
"special": 100.0,
"unmanned": 50.0
},
"alert_zone_radius": {
"aircraft": 50.0,
"special": 50.0,
"unmanned": 25.0
}
}
}
}

39
src/config/AirportBounds.h
View File

@ -2,47 +2,10 @@
#define AIRPORT_SPATIAL_AIRPORT_BOUNDS_H
#include "spatial/QuadTree.h"
#include "AreaConfig.h"
#include <string>
#include <unordered_map>
// 区域类型
enum class AreaType {
RUNWAY, // 跑道
TAXIWAY, // 滑行道
GATE, // 停机位
SERVICE, // 服务区
TEST_ZONE // 测试区
};
// 区域配置
struct AreaConfig {
double vehicleCollisionRadius = 0.0; // 车辆间冲突检测半径
double aircraftCollisionRadius = 0.0; // 航空器与车辆冲突检测半径
double heightThreshold = 0.0; // 高度阈值
double warning_zone_radius = 0.0; // 预警区域半径
double alert_zone_radius = 0.0; // 警报区域半径
// 获取不同类型交通工具间的告警阈值
struct CollisionThresholds {
double warning; // 预警距离
double critical; // 告警距离
};
// 计算两个交通工具之间的告警阈值
CollisionThresholds getThresholds(bool isAircraft1, bool isAircraft2) const {
// 如果没有配置预警和警报距离，使用碰撞半径作为默认值
double warningRadius = warning_zone_radius > 0 ? warning_zone_radius :
(isAircraft1 || isAircraft2 ? aircraftCollisionRadius : vehicleCollisionRadius);
double alertRadius = alert_zone_radius > 0 ? alert_zone_radius :
(isAircraft1 || isAircraft2 ? aircraftCollisionRadius * 0.5 : vehicleCollisionRadius * 0.5);
return {
warningRadius, // 预警距离
alertRadius // 警报距离
};
}
};
// 机场区域定义
class AirportBounds {
public:

53
src/config/AreaConfig.h Normal file
View File

@ -0,0 +1,53 @@
#ifndef AIRPORT_CONFIG_AREA_CONFIG_H
#define AIRPORT_CONFIG_AREA_CONFIG_H
#include "types/BasicTypes.h"
// 区域类型
enum class AreaType {
RUNWAY, // 跑道
TAXIWAY, // 滑行道
GATE, // 停机位
SERVICE, // 服务区
TEST_ZONE // 测试区
};
// 半径配置
struct RadiusConfig {
double aircraft; // 航空器半径
double special; // 特勤车半径
double unmanned; // 无人车半径
double getRadius(MovingObjectType type) const {
switch (type) {
case MovingObjectType::AIRCRAFT: return aircraft;
case MovingObjectType::SPECIAL: return special;
case MovingObjectType::UNMANNED: return unmanned;
}
return special; // 默认返回特勤车半径
}
};
// 区域配置
struct AreaConfig {
RadiusConfig collision_radius; // 碰撞半径
double height_threshold; // 高度阈值
RadiusConfig warning_zone_radius; // 预警区域半径
RadiusConfig alert_zone_radius; // 告警区域半径
// 获取碰撞阈值
struct CollisionThresholds {
double warning; // 预警距离
double critical; // 告警距离
};
// 根据物体类型获取对应的阈值
CollisionThresholds getThresholds(MovingObjectType type1, MovingObjectType type2) const {
return {
warning_zone_radius.getRadius(type1) + warning_zone_radius.getRadius(type2),
alert_zone_radius.getRadius(type1) + alert_zone_radius.getRadius(type2)
};
}
};
#endif // AIRPORT_CONFIG_AREA_CONFIG_H

602
src/detector/CollisionDetector.cpp
View File

@ -34,7 +34,18 @@ void CollisionDetector::updateTraffic(const std::vector<Aircraft>& aircraft,
const auto& bounds = airportBounds_.getAirportBounds();
if (vehicle.position.x >= bounds.x && vehicle.position.x <= (bounds.x + bounds.width) &&
vehicle.position.y >= bounds.y && vehicle.position.y <= (bounds.y + bounds.height)) {
vehicleTree_.insert(vehicle);
// 根据是否可控设置车辆类型
Vehicle updatedVehicle = vehicle;
if (controllableVehicles_->isControllable(vehicle.vehicleNo)) {
updatedVehicle.type = MovingObjectType::UNMANNED;
updatedVehicle.isControllable = true;
} else {
updatedVehicle.type = MovingObjectType::SPECIAL;
updatedVehicle.isControllable = false;
}
vehicleTree_.insert(updatedVehicle);
++validVehicles;
} else {
Logger::warning(
@ -70,27 +81,22 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
// 获取所有车辆
auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds());
// 可控车辆
std::vector<Vehicle> controlVehicles;
// 可控车辆（无人车）
std::vector<Vehicle> unmannedVehicles;
for (const auto& vehicle : allVehicles) {
if (controllableVehicles_->isControllable(vehicle.vehicleNo)) {
controlVehicles.push_back(vehicle);
if (vehicle.isControllable) {
unmannedVehicles.push_back(vehicle);
}
}
// 检测可控车辆与航空器的碰撞
for (const auto& aircraft : aircraftData_) {
for (const auto& vehicle : controlVehicles) {
for (const auto& vehicle : unmannedVehicles) {
// 检测碰撞
bool hasCollision = checkRectangleBasedAircraftVehicleCollision(
auto collisionResult = checkCollision(
aircraft, vehicle, 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);
if (collisionResult.willCollide) {
// 计算相对运动
MovementVector av(aircraft.speed, aircraft.heading);
MovementVector vv(vehicle.speed, vehicle.heading);
@ -98,64 +104,62 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
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);
// 根据相对运动方向调整相对速度符号
if (collisionResult.timeToCollision > 0) {
// 如果预测到未来会碰撞，说明当前在接近，相对速度应为负
relativeSpeed = -relativeSpeed;
}
// 评估风险等级
auto [level, zoneType] = evaluateRisk(
currentDistance,
collisionResult,
aircraft.position,
true, // aircraft
false, // vehicle
true // hasCollision
aircraft.type,
vehicle.type
);
if (level != RiskLevel::NONE) {
risks.push_back({
aircraft.flightNo,
vehicle.vehicleNo,
aircraft.id,
vehicle.id,
level,
currentDistance,
currentDistance, // 最小距离就是当前距离
collisionResult.minDistance,
collisionResult.minDistance,
relativeSpeed,
{vx, vy}, // 相对运动向量
zoneType
{vx, vy},
zoneType,
collisionResult.timeToCollision,
collisionResult.collisionPoint
});
Logger::debug("检测到航空器与车辆碰撞风险: aircraft=", aircraft.flightNo,
", vehicle=", vehicle.vehicleNo,
", distance=", currentDistance, "m, level=", static_cast<int>(level),
Logger::debug("检测到碰撞风险: obj1=", aircraft.id,
", obj2=", vehicle.id,
", minDistance=", collisionResult.minDistance, "m",
", timeToCollision=", collisionResult.timeToCollision, "s",
", collisionPoint=(", collisionResult.collisionPoint.x, ",",
collisionResult.collisionPoint.y, ")",
", level=", static_cast<int>(level),
", zone=", static_cast<int>(zoneType),
", relativeSpeed=", relativeSpeed, "m/s, relativeMotion=(", vx, ",", vy, ")",
", isMovingAway=", relativeSpeed > 0 ? "是" : "否");
", relativeSpeed=", relativeSpeed, "m/s",
", relativeMotion=(", vx, ",", vy, ")");
}
}
}
}
// 检测可控车辆与所有车辆的碰撞
for (size_t i = 0; i < controlVehicles.size(); ++i) {
const auto& vehicle1 = controlVehicles[i];
// 检查与所有其他车辆的碰撞
for (const auto& vehicle1 : unmannedVehicles) {
for (const auto& vehicle2 : allVehicles) {
// 跳过自身
if (vehicle1.vehicleNo == vehicle2.vehicleNo) {
if (vehicle1.id == vehicle2.id) {
continue;
}
// 检测碰撞
bool hasCollision = checkRectangleBasedVehicleCollision(
auto collisionResult = checkCollision(
vehicle1, vehicle2, 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);
if (collisionResult.willCollide) {
// 计算相对运动
MovementVector v1v(vehicle1.speed, vehicle1.heading);
MovementVector v2v(vehicle2.speed, vehicle2.heading);
@ -163,38 +167,44 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
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);
// 根据相对运动方向调整相对速度符号
if (collisionResult.timeToCollision > 0) {
// 如果预测到未来会碰撞，说明当前在接近，相对速度应为负
relativeSpeed = -relativeSpeed;
}
// 评估风险等级
auto [level, zoneType] = evaluateRisk(
currentDistance,
collisionResult,
vehicle1.position,
false, // vehicle
false, // vehicle
true // hasCollision
vehicle1.type,
vehicle2.type
);
if (level != RiskLevel::NONE) {
risks.push_back({
vehicle1.vehicleNo,
vehicle2.vehicleNo,
vehicle1.id,
vehicle2.id,
level,
currentDistance,
currentDistance, // 最小距离就是当前距离
collisionResult.minDistance,
collisionResult.minDistance,
relativeSpeed,
{vx, vy}, // 相对运动向量
zoneType
{vx, vy},
zoneType,
collisionResult.timeToCollision,
collisionResult.collisionPoint
});
Logger::debug("检测到车辆间碰撞风险: vehicle1=", vehicle1.vehicleNo,
", vehicle2=", vehicle2.vehicleNo,
", distance=", currentDistance, "m, level=", static_cast<int>(level),
Logger::debug("检测到碰撞风险: obj1=", vehicle1.id,
", obj2=", vehicle2.id,
", minDistance=", collisionResult.minDistance, "m",
", timeToCollision=", collisionResult.timeToCollision, "s",
", collisionPoint=(", collisionResult.collisionPoint.x, ",",
collisionResult.collisionPoint.y, ")",
", level=", static_cast<int>(level),
", zone=", static_cast<int>(zoneType),
", relativeSpeed=", relativeSpeed, "m/s, relativeMotion=(", vx, ",", vy, ")",
", isMovingAway=", relativeSpeed > 0 ? "是" : "否");
", relativeSpeed=", relativeSpeed, "m/s",
", relativeMotion=(", vx, ",", vy, ")");
}
}
}
@ -203,38 +213,40 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
return risks;
}
// 添加一个新的辅助函数来统一处理风险等级和预警区域的判断
// 统一的风险评估函数
std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
double currentDistance,
const collision::CollisionResult& collisionResult,
const Vector2D& position,
bool isAircraft1,
bool isAircraft2,
bool willCollide) const {
MovingObjectType type1,
MovingObjectType type2) const {
// 取区域配置
const auto& areaConfig = getCollisionParams(position);
auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2);
auto thresholds = areaConfig.getThresholds(type1, type2);
RiskLevel level = RiskLevel::NONE;
WarningZoneType zoneType = WarningZoneType::NONE;
// 如果预测到碰撞并且当前距离小于警报距离，设置为危险等级
if (willCollide && currentDistance <= thresholds.critical) {
level = RiskLevel::CRITICAL;
zoneType = WarningZoneType::DANGER;
}
// 如果预测到碰撞并且当前距离在预警范围内，设置为预警等级
else if (willCollide && currentDistance <= thresholds.warning) {
level = RiskLevel::WARNING;
zoneType = WarningZoneType::WARNING;
// 如果预测到碰撞，只根据距离判断风险等级
if (collisionResult.willCollide) {
// 当前距离小于警报距离 -> 危险等级
if (collisionResult.minDistance <= thresholds.critical) {
level = RiskLevel::CRITICAL;
zoneType = WarningZoneType::DANGER;
}
// 当前距离在预警范围内 -> 预警等级
else if (collisionResult.minDistance <= thresholds.warning) {
level = RiskLevel::WARNING;
zoneType = WarningZoneType::WARNING;
}
}
// 记录调试信息
Logger::debug(
"风险评估: 当前距离=", currentDistance,
"风险评估: 当前最小距离=", collisionResult.minDistance,
"m, 预警阈值=", thresholds.warning,
"m, 警报阈值=", thresholds.critical,
"m, 预测碰撞=", willCollide ? "是" : "否",
"m, 预测碰撞=", collisionResult.willCollide ? "是" : "否",
", 风险等级=", static_cast<int>(level),
", 区域类型=", static_cast<int>(zoneType)
);
@ -242,50 +254,406 @@ std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
return {level, zoneType};
}
bool CollisionDetector::checkRectangleBasedAircraftVehicleCollision(
const Aircraft& aircraft,
const Vehicle& vehicle,
// 统一的碰撞检测函数
collision::CollisionResult CollisionDetector::checkCollision(
const MovingObject& obj1,
const MovingObject& obj2,
double timeWindow) const {
// 创建矩形
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)}
};
// 获取区域配置
const auto& areaConfig = getCollisionParams(obj1.position);
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)}
};
// 获取各自的碰撞半径和警告半径
double radius1 = areaConfig.collision_radius.getRadius(obj1.type);
double radius2 = areaConfig.collision_radius.getRadius(obj2.type);
double warning_radius1 = areaConfig.warning_zone_radius.getRadius(obj1.type);
double warning_radius2 = areaConfig.warning_zone_radius.getRadius(obj2.type);
// 检测碰撞
return collision::predictRectangleBasedCollision(rect1, rect2, timeWindow);
// 计算当前距离
double dx = obj2.position.x - obj1.position.x;
double dy = obj2.position.y - obj1.position.y;
double current_distance = std::sqrt(dx*dx + dy*dy);
// 如果当前距离大于警告区域，直接返回不会碰撞
if (current_distance > (warning_radius1 + warning_radius2)) {
collision::CollisionResult result;
result.willCollide = false;
result.minDistance = current_distance;
return result;
}
// 否则进行碰撞检测
return predictCircleBasedCollision(
obj1.position, radius1, obj1.speed, obj1.heading,
obj2.position, radius2, obj2.speed, obj2.heading,
timeWindow
);
}
bool CollisionDetector::checkRectangleBasedVehicleCollision(
const Vehicle& v1,
const Vehicle& v2,
collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
const Vector2D& pos1, double radius1, double speed1, double heading1,
const Vector2D& pos2, double radius2, double speed2, double heading2,
double timeWindow) const {
// 创建矩形
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)}
};
collision::CollisionResult result;
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)}
};
// 计算安全距离（两个圆的半径之和）
double safe_distance = radius1 + radius2;
// 检测碰撞
return collision::predictRectangleBasedCollision(rect1, rect2, timeWindow);
// 1. 计算当前距离
double dx = pos2.x - pos1.x;
double dy = pos2.y - pos1.y;
double current_distance = std::sqrt(dx*dx + dy*dy);
// 初始化最小距离
result.minDistance = current_distance;
result.timeToMinDistance = 0.0;
// 2. 计算速度分量
double vx1 = speed1 * std::sin((90.0 - heading1) * M_PI / 180.0);
double vy1 = speed1 * std::cos((90.0 - heading1) * M_PI / 180.0);
double vx2 = speed2 * std::sin((90.0 - heading2) * M_PI / 180.0);
double vy2 = speed2 * std::cos((90.0 - heading2) * M_PI / 180.0);
// 3. 计算相对运动
double rel_vx = vx2 - vx1;
double rel_vy = vy2 - vy1;
double rel_speed = std::sqrt(rel_vx*rel_vx + rel_vy*rel_vy);
// 计算相对运动方向与连线方向的夹角
double motion_angle = std::atan2(rel_vy, rel_vx);
double connection_angle = std::atan2(dy, dx);
double angle = std::abs(motion_angle - connection_angle);
if (angle > M_PI) {
angle = 2 * M_PI - angle; // 确保夹角不超过180度
}
// 4. 确定碰撞类型
if (speed1 < 0.1 && speed2 < 0.1) {
result.type = collision::CollisionType::STATIC;
if (current_distance <= safe_distance) {
result.willCollide = true;
result.timeToCollision = 0.0;
result.minDistance = current_distance;
result.timeToMinDistance = 0.0;
result.collisionPoint = {
(pos1.x + pos2.x) / 2.0,
(pos1.y + pos2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(pos1, speed1, heading1);
result.object2State = collision::CollisionObjectState(pos2, speed2, heading2);
}
return result;
}
double angle_diff = getAngleDifference(heading1, heading2);
if (angle_diff > 150.0) {
result.type = collision::CollisionType::HEAD_ON;
// 对于相向运动，直接计算碰撞时间
double closing_speed = speed1 + speed2; // 相对接近速度是两个速度之和
if (closing_speed > 0.1) { // 避免除以零
double collision_distance = current_distance - safe_distance;
if (collision_distance <= 0) {
result.willCollide = true;
result.timeToCollision = 0.0;
result.minDistance = current_distance;
result.timeToMinDistance = 0.0;
result.collisionPoint = {
(pos1.x + pos2.x) / 2.0,
(pos1.y + pos2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(pos1, speed1, heading1);
result.object2State = collision::CollisionObjectState(pos2, speed2, heading2);
} else {
result.timeToCollision = collision_distance / closing_speed;
result.willCollide = result.timeToCollision <= timeWindow;
if (result.willCollide) {
result.minDistance = safe_distance;
result.timeToMinDistance = result.timeToCollision;
// 计算碰撞时两个物体的实际位置
double move_distance1 = speed1 * result.timeToCollision;
double move_distance2 = speed2 * result.timeToCollision;
// 计算物体1的碰撞位置（向前移动到碰撞点前方）
Vector2D collision1 = {
pos1.x + move_distance1 * std::sin(heading1 * M_PI / 180.0),
pos1.y + move_distance1 * std::cos(heading1 * M_PI / 180.0)
};
// 计算物体2的碰撞位置（也是向前移动）
Vector2D collision2 = {
pos2.x + move_distance2 * std::sin(heading2 * M_PI / 180.0),
pos2.y + move_distance2 * std::cos(heading2 * M_PI / 180.0)
};
// 碰撞点在两个物体的中点
result.collisionPoint = {
(collision1.x + collision2.x) / 2.0,
(collision1.y + collision2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(collision1, speed1, heading1);
result.object2State = collision::CollisionObjectState(collision2, speed2, heading2);
}
}
return result;
}
} else if (angle_diff > 15.0 && angle_diff < 165.0) { // 放宽交叉路径的角度范围
result.type = collision::CollisionType::CROSSING;
Logger::debug(
"交叉路径检测: angle_diff=", angle_diff,
", pos1=(", pos1.x, ",", pos1.y, ")",
", pos2=(", pos2.x, ",", pos2.y, ")",
", v1=(", vx1, ",", vy1, ")",
", v2=(", vx2, ",", vy2, ")",
", rel_v=(", rel_vx, ",", rel_vy, ")",
", current_distance=", current_distance,
", safe_distance=", safe_distance
);
// 计算两条轨迹的最近点时间
// 参考：https://geomalgorithms.com/a07-_distance.html
double a = vx1 * vx1 + vy1 * vy1; // v1 · v1
double b = -(vx1 * vx2 + vy1 * vy2); // -v1 · v2
double c = vx2 * vx2 + vy2 * vy2; // v2 · v2
double d = vx1 * dx + vy1 * dy; // v1 · w
double e = -(vx2 * dx + vy2 * dy); // -v2 · w
double det = a * c - b * b; // 行列式
Logger::debug(
"参数计算: a=", a,
", b=", b,
", c=", c,
", d=", d,
", e=", e,
", det=", det
);
// 对于交叉路径，们可以直接计算交点时间
// 解方程组：
// x1 + vx1 * t = x2 + vx2 * t
// y1 + vy1 * t = y2 + vy2 * t
double t_cross;
if (std::abs(vx1 - vx2) > 0.1) {
t_cross = (pos2.x - pos1.x) / (vx1 - vx2);
} else {
t_cross = (pos2.y - pos1.y) / (vy1 - vy2);
}
Logger::debug(
"交叉时间计算: t_cross=", t_cross
);
if (t_cross >= 0 && t_cross <= timeWindow) {
// 计算交叉点
Vector2D cross_point = {
pos1.x + vx1 * t_cross,
pos1.y + vy1 * t_cross
};
// 由于有碰撞半径，实际碰撞会提前发生
// 对于交叉路径，两车需要各自移动 safe_distance/√2 的距离才会相遇
double offset_distance = safe_distance / std::sqrt(2.0);
double offset_time = offset_distance / speed1; // 两车速度相同，用任意一个都以
double collision_time = t_cross - offset_time;
Logger::debug(
"碰撞时间计算: t_cross=", t_cross,
", safe_distance=", safe_distance,
", offset_distance=", offset_distance,
", offset_time=", offset_time,
", collision_time=", collision_time
);
if (collision_time >= 0) {
result.willCollide = true;
result.timeToCollision = collision_time;
result.minDistance = safe_distance;
result.timeToMinDistance = collision_time;
// 计算碰撞点（在两个物体的中间）
Vector2D collision1 = {
pos1.x + vx1 * collision_time,
pos1.y + vy1 * collision_time
};
Vector2D collision2 = {
pos2.x + vx2 * collision_time,
pos2.y + vy2 * collision_time
};
// 碰撞点在两车连线的中点
result.collisionPoint = {
(collision1.x + collision2.x) / 2.0,
(collision1.y + collision2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(
collision1, speed1, heading1);
result.object2State = collision::CollisionObjectState(
collision2, speed2, heading2);
Logger::debug(
"交叉路径碰撞: collision_time=", collision_time,
", collision1=(", collision1.x, ",", collision1.y, ")",
", collision2=(", collision2.x, ",", collision2.y, ")",
", collision_point=(", result.collisionPoint.x,
",", result.collisionPoint.y, ")"
);
return result;
}
}
} else {
result.type = collision::CollisionType::PARALLEL;
// 平行运动判断：
// 1. 航向差很小（小于30度）
// 2. 横向距离大于安全距离
// 3. 相对速度很小（说明速度接近）
if (angle_diff < 30.0) {
// 计算垂直于运动方向的横向距离
// 将航向角转换为学坐标系中的旋转角度（逆时针为正）
double rotation_angle = (90.0 - heading1) * M_PI / 180.0;
// 使用标准的二维坐标旋转公式
double dx_rotated = dx * std::cos(rotation_angle) - dy * std::sin(rotation_angle);
double dy_rotated = dx * std::sin(rotation_angle) + dy * std::cos(rotation_angle);
// 横向距离就是旋转后的y坐标的绝对值
double lateral_distance = std::abs(dy_rotated);
Logger::debug(
"平行运动检测: angle_diff=", angle_diff,
", heading1=", heading1,
", rotation_angle=", rotation_angle * 180.0 / M_PI,
", dx=", dx,
", dy=", dy,
", dx_rotated=", dx_rotated,
", dy_rotated=", dy_rotated,
", lateral_distance=", lateral_distance,
", safe_distance=", safe_distance,
", rel_speed=", rel_speed
);
// 如果横向距离大于等于安全距离，且相对速度很小，则不会碰撞
if (lateral_distance >= safe_distance && rel_speed < 1.0) {
result.willCollide = false;
return result;
}
}
}
// 5. 在时间窗口内预测碰撞
const int STEPS = 120; // 增加采样点数以提高精度
double dt = timeWindow / STEPS; // 时间步长
// 如果当前已经碰撞
if (current_distance <= safe_distance) {
result.willCollide = true;
result.timeToCollision = 0.0;
result.minDistance = current_distance;
result.timeToMinDistance = 0.0;
result.collisionPoint = {
(pos1.x + pos2.x) / 2.0,
(pos1.y + pos2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(pos1, speed1, heading1);
result.object2State = collision::CollisionObjectState(pos2, speed2, heading2);
return result;
}
// 在时间窗口内采样检查
double prev_distance = current_distance;
Logger::debug(
"开始时间窗口采样: STEPS=", STEPS,
", dt=", dt,
", timeWindow=", timeWindow,
", safe_distance=", safe_distance,
", current_distance=", current_distance
);
for (int i = 1; i <= STEPS; ++i) {
double t = i * dt;
// 计算t时的位置
Vector2D future1 = {
pos1.x + vx1 * t,
pos1.y + vy1 * t
};
Vector2D future2 = {
pos2.x + vx2 * t,
pos2.y + vy2 * t
};
// 计算t时刻的距离
double dx_t = future2.x - future1.x;
double dy_t = future2.y - future1.y;
double distance = std::sqrt(dx_t*dx_t + dy_t*dy_t);
Logger::debug(
"采样点状态: step=", i,
", t=", t,
", distance=", distance,
", prev_distance=", prev_distance,
", min_distance=", result.minDistance
);
// 更新小距离
if (distance < result.minDistance) {
result.minDistance = distance;
result.timeToMinDistance = t;
Logger::debug(
"更新最小距离: min_distance=", distance,
", time_to_min=", t
);
}
// 检查是否会碰撞
if (distance <= safe_distance) {
result.willCollide = true;
// 使用线性插值计算更精确的碰撞时间
double t_interp = t - dt + (dt * (safe_distance - distance) / (prev_distance - distance));
result.timeToCollision = t_interp;
Logger::debug(
"找到碰撞: t=", t,
", distance=", distance,
", prev_distance=", prev_distance,
", dt=", dt,
", t_interp=", t_interp,
", future1=(", future1.x, ",", future1.y, ")",
", future2=(", future2.x, ",", future2.y, ")"
);
// 使用插值时间计算更精确的碰撞点
Vector2D interp1 = {
pos1.x + vx1 * t_interp,
pos1.y + vy1 * t_interp
};
Vector2D interp2 = {
pos2.x + vx2 * t_interp,
pos2.y + vy2 * t_interp
};
result.collisionPoint = {
(interp1.x + interp2.x) / 2.0,
(interp1.y + interp2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(interp1, speed1, heading1);
result.object2State = collision::CollisionObjectState(interp2, speed2, heading2);
break;
}
prev_distance = distance;
// 如果相对速度很小，且距离增加，可以提前退出
if (rel_speed < 0.1 && i > 1) {
if (distance > prev_distance) {
break;
}
}
}
return result;
}

64
src/detector/CollisionDetector.h
View File

@ -6,7 +6,7 @@
#include "config/AirportBounds.h"
#include "vehicle/ControllableVehicles.h"
#include "config/SystemConfig.h"
#include "RectangleCollision.h"
#include "CollisionTypes.h"
#include "utils/Logger.h"
#include <vector>
@ -33,6 +33,8 @@ struct CollisionRisk {
double relativeSpeed; // 相对速度
Vector2D relativeMotion; // 相对运动向量
WarningZoneType zoneType; // 预警区域类型
double timeToCollision; // 预计碰撞时间（秒）
Vector2D collisionPoint; // 预计碰撞点
};
class CollisionDetector {
@ -46,19 +48,11 @@ public:
// 检测所有可能的碰撞
std::vector<CollisionRisk> detectCollisions();
// 基于矩形的航空器和车辆碰撞检测
bool checkRectangleBasedAircraftVehicleCollision(
const Aircraft& aircraft,
const Vehicle& vehicle,
double timeWindow = 30.0
) const;
// 基于矩形的车辆间碰撞检测
bool checkRectangleBasedVehicleCollision(
const Vehicle& v1,
const Vehicle& v2,
double timeWindow = 30.0
) const;
// 统一的碰撞检测函数
collision::CollisionResult checkCollision(
const MovingObject& obj1,
const MovingObject& obj2,
double timeWindow = 30.0) const;
private:
const AirportBounds& airportBounds_;
@ -66,6 +60,21 @@ private:
std::vector<Aircraft> aircraftData_;
const ControllableVehicles* controllableVehicles_;
// 将角度标准化到[0, 360)范围
static double normalizeAngle(double angle) {
while (angle >= 360.0) angle -= 360.0;
while (angle < 0.0) angle += 360.0;
return angle;
}
// 计算两个角度之间的最小差值（范围[0, 180]）
static double getAngleDifference(double angle1, double angle2) {
angle1 = normalizeAngle(angle1);
angle2 = normalizeAngle(angle2);
double diff = std::abs(angle1 - angle2);
return std::min(diff, 360.0 - diff);
}
// 根据区域获取碰撞检测参数
AreaConfig getCollisionParams(const Vector2D& position) const {
auto areaType = airportBounds_.getAreaType(position);
@ -74,11 +83,10 @@ private:
// 统一的风险评估函数
std::pair<RiskLevel, WarningZoneType> evaluateRisk(
double currentDistance,
const collision::CollisionResult& collisionResult,
const Vector2D& position,
bool isAircraft1,
bool isAircraft2,
bool willCollide) const;
MovingObjectType type1,
MovingObjectType type2) const;
// 计算相对运动
struct MovementVector {
@ -91,12 +99,24 @@ private:
// 计算速度分量
vx = speed * std::sin(radians); // 东向分量
vy = speed * std::cos(radians); // 北向分量
// 记录调试信息
Logger::debug("速度分量计算: speed=", speed, "m/s, heading=", heading,
"°, vx=", vx, "m/s, vy=", vy, "m/s");
}
};
// 预测位置的辅助函数
Vector2D predictPosition(const Vector2D& position, double speed, double heading, double t) const {
double vx = speed * std::sin(heading * M_PI / 180.0);
double vy = speed * std::cos(heading * M_PI / 180.0);
return {
position.x + vx * t,
position.y + vy * t
};
}
// 基于圆形的碰撞检测核心函数
collision::CollisionResult predictCircleBasedCollision(
const Vector2D& pos1, double radius1, double speed1, double heading1,
const Vector2D& pos2, double radius2, double speed2, double heading2,
double timeWindow) const;
};
#endif // AIRPORT_DETECTOR_COLLISION_DETECTOR_H

28
src/detector/CollisionTypes.h
View File

@ -4,6 +4,7 @@
#define AIRPORT_DETECTOR_COLLISION_TYPES_H
#include "types/BasicTypes.h"
#include <limits>
namespace collision {
@ -15,13 +16,27 @@ enum class CollisionType {
PARALLEL // 平行运动
};
// 碰撞检测结果
// 碰撞时刻的物体状态
struct CollisionObjectState {
Vector2D position; // 位置
double speed; // 速度
double heading; // 航向
CollisionObjectState() : speed(0.0), heading(0.0) {}
CollisionObjectState(const Vector2D& pos, double spd, double hdg)
: position(pos), speed(spd), heading(hdg) {}
};
// 碰撞预测结果
struct CollisionResult {
bool willCollide; // 是否会碰撞
double timeToCollision; // 碰撞时间
Vector2D collisionPoint; // 碰撞点
double minDistance; // 最小距离
CollisionType type; // 碰撞类型
bool willCollide; // 是否会碰撞
double timeToCollision; // 碰撞时间（从当前时刻开始的秒数）
Vector2D collisionPoint; // 碰撞点位置
double minDistance; // 最小距离
double timeToMinDistance; // 到达最小距离的时间（从当前时刻开始的秒数）
CollisionType type; // 碰撞类型
CollisionObjectState object1State; // 物体1在碰撞时刻的状态
CollisionObjectState object2State; // 物体2在碰撞时刻的状态
// 构造函数
CollisionResult() :
@ -29,6 +44,7 @@ struct CollisionResult {
timeToCollision(std::numeric_limits<double>::infinity()),
collisionPoint({0, 0}),
minDistance(std::numeric_limits<double>::infinity()),
timeToMinDistance(std::numeric_limits<double>::infinity()),
type(CollisionType::STATIC) {}
};

2
src/spatial/CoordinateConverter.h
View File

@ -1,7 +1,7 @@
#ifndef AIRPORT_SPATIAL_COORDINATE_CONVERTER_H
#define AIRPORT_SPATIAL_COORDINATE_CONVERTER_H
#include "../types/BasicTypes.h"
#include "types/BasicTypes.h"
#include <cmath>
#include <utility>

18
src/types/BasicTypes.h
View File

@ -4,6 +4,13 @@
#include <deque>
#include <cstdint>
// 移动物体类型
enum class MovingObjectType {
AIRCRAFT, // 航空器
SPECIAL, // 特勤车
UNMANNED // 无人车（可控车辆）
};
// 基础数据类型
struct Vector2D {
double x;
@ -28,8 +35,8 @@ struct PositionRecord {
// 移动物体基类
class MovingObject {
public:
MovingObject() : heading(0.0), speed(0.0), timestamp(0) {} // 添加构造函数
virtual ~MovingObject() = default; // 添加虚析构函数以支持 dynamic_cast
MovingObject() : heading(0.0), speed(0.0), timestamp(0), type(MovingObjectType::SPECIAL) {}
virtual ~MovingObject() = default;
std::string id;
GeoPosition geo;
@ -37,6 +44,7 @@ public:
double heading;
double speed;
uint64_t timestamp;
MovingObjectType type; // 添加类型字段
std::deque<PositionRecord> positionHistory;
@ -57,6 +65,8 @@ public:
// 航空器类
class Aircraft : public MovingObject {
public:
Aircraft() { type = MovingObjectType::AIRCRAFT; }
std::string flightNo;
std::string trackNumber;
double altitude;
@ -71,8 +81,10 @@ public:
// 车辆类
class Vehicle : public MovingObject {
public:
Vehicle() { type = MovingObjectType::SPECIAL; } // 默认为特勤车
std::string vehicleNo;
bool controllable;
bool isControllable; // true 表示无人车，false 表示特勤车
bool isValidPosition(const GeoPosition& newPos) const override;
bool isValidSpeed(double speed) const override;

306
tests/BasicCollisionTest.cpp Normal file
View File

@ -0,0 +1,306 @@
#include "detector/CollisionDetector.h"
#include "vehicle/ControllableVehicles.h"
#include "config/AirportBounds.h"
#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include "utils/Logger.h"
// Mock ControllableVehicles 类
class MockControllableVehicles : public ControllableVehicles {
public:
MockControllableVehicles() : ControllableVehicles("") {}
MOCK_METHOD(bool, isControllable, (const std::string& vehicleNo), (const));
};
// Mock AirportBounds 类
class MockAirportBounds : public AirportBounds {
public:
MockAirportBounds() : AirportBounds("") {
// 设置测试区域边界
airportBounds_ = Bounds(0, 0, 5000, 4000);
areaBounds_[AreaType::TEST_ZONE] = airportBounds_;
// 设置测试区域配置
AreaConfig config;
config.collision_radius = {50.0, 50.0, 25.0}; // aircraft, special, unmanned
config.height_threshold = 10.0;
config.warning_zone_radius = {100.0, 100.0, 50.0};
config.alert_zone_radius = {50.0, 50.0, 25.0};
areaConfigs_[AreaType::TEST_ZONE] = config;
}
MOCK_METHOD(AreaType, getAreaType, (const Vector2D& position), (const));
const AreaConfig& getAreaConfig(AreaType type) const override {
auto it = areaConfigs_.find(type);
if (it == areaConfigs_.end()) {
throw std::runtime_error("Invalid area type");
}
return it->second;
}
};
class BasicCollisionTest : public ::testing::Test {
protected:
void SetUp() override {
airportBounds_ = std::make_unique<MockAirportBounds>();
mockControllableVehicles_ = std::make_unique<MockControllableVehicles>();
// 设置 Mock 对象的行为
EXPECT_CALL(*airportBounds_, getAreaType(::testing::_))
.WillRepeatedly(::testing::Return(AreaType::TEST_ZONE));
detector_ = std::make_unique<CollisionDetector>(*airportBounds_, *mockControllableVehicles_);
}
std::unique_ptr<MockAirportBounds> airportBounds_;
std::unique_ptr<MockControllableVehicles> mockControllableVehicles_;
std::unique_ptr<CollisionDetector> detector_;
};
// 1. 静态碰撞测试
TEST_F(BasicCollisionTest, StaticCollision) {
// 创建两个静止且重叠的物体
Vehicle v1;
v1.vehicleNo = "V1";
v1.position = {100.0, 100.0};
v1.speed = 0.0;
v1.heading = 0.0;
v1.type = MovingObjectType::UNMANNED;
Vehicle v2;
v2.vehicleNo = "V2";
v2.position = {120.0, 100.0}; // 距离20米，小于碰撞半径25米
v2.speed = 0.0;
v2.heading = 0.0;
v2.type = MovingObjectType::UNMANNED;
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(result.willCollide) << "距离小于碰撞半径的静止物体应该检测为碰撞";
EXPECT_DOUBLE_EQ(result.timeToCollision, 0.0) << "静止物体的碰撞时间应该为0";
EXPECT_EQ(result.type, collision::CollisionType::STATIC) << "应该识别为静态碰撞";
// 增加更多验证
EXPECT_DOUBLE_EQ(result.minDistance, 20.0) << "最小距离应该是当前距离20米";
EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "静止物体的最小距离时间应该为0";
EXPECT_NEAR(result.collisionPoint.x, 110.0, 0.1) << "碰撞点应该在两车中点";
EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上";
// 验证物体状态
EXPECT_NEAR(result.object1State.position.x, 100.0, 0.1);
EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
EXPECT_DOUBLE_EQ(result.object1State.speed, 0.0);
EXPECT_DOUBLE_EQ(result.object1State.heading, 0.0);
EXPECT_NEAR(result.object2State.position.x, 120.0, 0.1);
EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
EXPECT_DOUBLE_EQ(result.object2State.speed, 0.0);
EXPECT_DOUBLE_EQ(result.object2State.heading, 0.0);
}
// 2. 相向碰撞测试
TEST_F(BasicCollisionTest, HeadOnCollision) {
// 创建两个相向运动的物体
Vehicle v1;
v1.vehicleNo = "V1";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.heading = 90.0; // 向东
v1.type = MovingObjectType::UNMANNED;
Vehicle v2;
v2.vehicleNo = "V2";
v2.position = {200.0, 100.0}; // 在v1东边100米
v2.speed = 10.0;
v2.heading = 270.0; // 向西
v2.type = MovingObjectType::UNMANNED;
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(result.willCollide) << "相向运动的物体应该检测为碰撞";
EXPECT_NEAR(result.timeToCollision, 2.5, 0.1) << "碰撞时间应该接近2.5秒((100-50)米/20米每秒)";
EXPECT_EQ(result.type, collision::CollisionType::HEAD_ON) << "应该识别为相向碰撞";
EXPECT_NEAR(result.collisionPoint.x, 150.0, 0.1) << "碰撞点应该在两车中点处(100+200)/2=150";
EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上";
// 增加更多验证
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
EXPECT_NEAR(result.timeToMinDistance, 2.5, 0.1) << "最小距离时间应该等于碰撞时间";
// 验证物体状态
EXPECT_NEAR(result.object1State.position.x, 125.0, 0.1);
EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
EXPECT_NEAR(result.object2State.position.x, 175.0, 0.1);
EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
EXPECT_DOUBLE_EQ(result.object2State.heading, 270.0);
}
// 3. 平行运动测试
TEST_F(BasicCollisionTest, ParallelMotion) {
// 创建两个平行运动的物体
Vehicle v1;
v1.vehicleNo = "V1";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.heading = 90.0; // 向东
v1.type = MovingObjectType::UNMANNED;
Vehicle v2;
v2.vehicleNo = "V2";
v2.position = {100.0, 150.0}; // 在v1北边50米，大于碰撞半径
v2.speed = 10.0;
v2.heading = 90.0; // 向东
v2.type = MovingObjectType::UNMANNED;
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_FALSE(result.willCollide) << "平行运动且距离大于碰撞半径的物体不应该检测为碰撞";
EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行<EFBFBD><EFBFBD>动";
// 增加更多验证
EXPECT_DOUBLE_EQ(result.minDistance, 50.0) << "最小距离应该是初始距离50米";
EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "平行运动的最小距离时间应该为0";
EXPECT_DOUBLE_EQ(result.timeToCollision, std::numeric_limits<double>::infinity()) << "平行运动不会碰撞";
// 不验证碰撞点，因为不会发生碰撞
}
// 4. 交叉路径测试
TEST_F(BasicCollisionTest, CrossingPaths) {
// 创建两个垂直交叉运动的物体
Vehicle v1;
v1.vehicleNo = "V1";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.heading = 75.0; // 向东北偏东方向运动
v1.type = MovingObjectType::UNMANNED;
Vehicle v2;
v2.vehicleNo = "V2";
v2.position = {150.0, 150.0};
v2.speed = 10.0;
v2.heading = 105.0; // 向东南偏东方向运动，与 v1 夹角为 30 度
v2.type = MovingObjectType::UNMANNED;
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(result.willCollide) << "交叉路径的物体应该检测为碰撞";
EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
// 计算碰撞时间和位置：
// v1: 速度分量 (2.59, 9.66) m/s
// v2: 速度分量 (-2.59, 9.66) m/s
// 相对速度: (-5.18, 0) m/s
// 相对速度大小: 5.18 m/s
// 碰撞点在两车轨迹交点前的安全距离处
double collision_time = 6.12; // 根据实际计算得到
Vector2D collision_point = {125.0, 184.15}; // 根据实际计算得到
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，撞时间应该接近6.12秒";
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在125.0";
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在184.15";
// 增加更多验证
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间";
// 验证物体状态
EXPECT_NEAR(result.object1State.position.x, 115.85, 0.1);
EXPECT_NEAR(result.object1State.position.y, 159.15, 0.1);
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
EXPECT_DOUBLE_EQ(result.object1State.heading, 75.0);
EXPECT_NEAR(result.object2State.position.x, 134.15, 0.1);
EXPECT_NEAR(result.object2State.position.y, 209.15, 0.1);
EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
EXPECT_DOUBLE_EQ(result.object2State.heading, 105.0);
}
// 5. 垂直交叉路径测试
TEST_F(BasicCollisionTest, PerpendicularCrossingPaths) {
// 创建两个垂直交叉运动的物体
Vehicle v1;
v1.vehicleNo = "V1";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.heading = 90.0; // 向东运动
v1.type = MovingObjectType::UNMANNED;
Vehicle v2;
v2.vehicleNo = "V2";
v2.position = {150.0, 150.0};
v2.speed = 10.0;
v2.heading = 180.0; // 向南运动，与 v1 夹角为 90 度
v2.type = MovingObjectType::UNMANNED;
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(result.willCollide) << "垂直交叉路径的物体应该检测为碰撞";
EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
// 计算碰撞时间和位置：
// v1: 速度分量 (0.0, 10.0) m/s
// v2: 速度分量 (-10.0, 0.0) m/s
// 相对速度: (-10.0, -10.0) m/s
// 相对速度大小: 14.14 m/s
// 由于碰撞半径为 25m，实际碰撞会提前发生
double collision_time = 1.46; // 根据实际计算得到
Vector2D collision_point = {117.68, 132.32}; // 根据实际计算得到
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，碰时间应该接近1.46秒";
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在117.68";
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在132.32";
// 增加更多验证
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间";
// 验证物体状态
EXPECT_NEAR(result.object1State.position.x, 100.0, 0.1);
EXPECT_NEAR(result.object1State.position.y, 114.64, 0.1);
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
EXPECT_NEAR(result.object2State.position.x, 135.36, 0.1);
EXPECT_NEAR(result.object2State.position.y, 150.0, 0.1);
EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
EXPECT_DOUBLE_EQ(result.object2State.heading, 180.0);
}
TEST_F(BasicCollisionTest, DivergentMotion) {
// 设置两个物体背向运动的场景
// 物体1在(150,100)，向右运动，航向90度
// 物体2在(200,100)，向右运动，航向90度
// 两个物体都在远离对方，不应该发生碰撞
Vehicle obj1;
obj1.vehicleNo = "V1";
obj1.position = {150, 100};
obj1.speed = 10;
obj1.heading = 90; // <20><>右运动
obj1.type = MovingObjectType::UNMANNED;
Vehicle obj2;
obj2.vehicleNo = "V2";
obj2.position = {300, 100}; // 增加初始距离到 150m
obj2.speed = 10;
obj2.heading = 90; // 也向右运动
obj2.type = MovingObjectType::SPECIAL;
auto result = detector_->checkCollision(obj1, obj2, 10.0);
// 验证结果
EXPECT_FALSE(result.willCollide) << "同向运动且距离大于安全距离的物体不应该发生碰撞";
EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
// 初始距离应该是 150m
EXPECT_NEAR(result.minDistance, 150.0, 0.1);
// 由于两车都以相同速度向右运动，最小距离应该保持不变
EXPECT_NEAR(result.timeToMinDistance, 0.0, 0.1);
}

1
tests/CMakeLists.txt
View File

@ -1,5 +1,6 @@
# 设置测试源文件
set(TEST_SOURCES
BasicCollisionTest.cpp
CollisionDetectorTest.cpp
BasicTypesTest.cpp
HTTPDataSourceTest.cpp

701
tests/CollisionDetectorTest.cpp
View File

@ -40,21 +40,27 @@ public:
class MockAirportBounds : public AirportBounds {
public:
MockAirportBounds() : AirportBounds("") {
// 设置更大的测试边界，以包含所有测试数据
airportBounds_ = Bounds(0, 0, 4000, 2000); // 4000x2000 的测试区域
Logger::info("MockAirportBounds initialized with bounds: ",
"x=", airportBounds_.x, ", y=", airportBounds_.y,
", width=", airportBounds_.width, ", height=", airportBounds_.height);
// 设置测试区域边界
airportBounds_ = Bounds(0, 0, 5000, 4000);
areaBounds_[AreaType::TEST_ZONE] = airportBounds_;
// 设置测试区域配置
AreaConfig config;
config.collision_radius = {50.0, 50.0, 25.0}; // aircraft, special, unmanned
config.height_threshold = 10.0;
config.warning_zone_radius = {100.0, 100.0, 50.0};
config.alert_zone_radius = {50.0, 50.0, 25.0};
areaConfigs_[AreaType::TEST_ZONE] = config;
}
// 覆盖原有方法，返回测试用的配置
AreaType getAreaType(const Vector2D& position) const override {
return AreaType::RUNWAY; // 简化测试，总是返回跑道区域
}
MOCK_METHOD(AreaType, getAreaType, (const Vector2D& position), (const));
const AreaConfig& getAreaConfig(AreaType type) const override {
static const AreaConfig config{50.0, 100.0, 15.0};
return config;
auto it = areaConfigs_.find(type);
if (it == areaConfigs_.end()) {
throw std::runtime_error("Invalid area type");
}
return it->second;
}
const Bounds& getAirportBounds() const override {
@ -72,6 +78,10 @@ protected:
airportBounds_ = std::make_unique<MockAirportBounds>();
mockControllableVehicles_ = std::make_unique<MockControllableVehicles>();
// 设置 Mock 对象的行为
EXPECT_CALL(*airportBounds_, getAreaType(::testing::_))
.WillRepeatedly(::testing::Return(AreaType::TEST_ZONE));
// 创建冲突检测器
detector_ = std::make_unique<CollisionDetector>(*airportBounds_, *mockControllableVehicles_);
}
@ -85,509 +95,254 @@ protected:
std::unique_ptr<CollisionDetector> detector_;
};
// 测试可控车辆与航空器的冲突检测
TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) {
// 设置 Mock 期望 - 在创建数据之前设置
EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH001"))
.WillRepeatedly(testing::Return(true));
Logger::info("Set mock expectation: VEH001 is controllable");
// 测试可控车辆（无人车）与航空器的碰撞检测
TEST_F(CollisionDetectorTest, UnmannedVehicleAircraftCollision) {
// 设置测试数据
Aircraft aircraft;
aircraft.flightNo = "TEST001";
aircraft.position = {100, 100};
aircraft.speed = 10;
aircraft.heading = 90;
Logger::info("Created aircraft: flightNo=", aircraft.flightNo,
", position=(", aircraft.position.x, ", ", aircraft.position.y, ")");
Vehicle vehicle;
vehicle.vehicleNo = "VEH001";
vehicle.position = {120, 100}; // <20><><EFBFBD>离航空器20米
vehicle.speed = 5;
vehicle.heading = 270;
Logger::info("Created vehicle: vehicleNo=", vehicle.vehicleNo,
", position=(", vehicle.position.x, ", ", vehicle.position.y, ")");
// 更新交通数据
detector_->updateTraffic({aircraft}, {vehicle});
Logger::info("Updated traffic data");
// 执行冲突检测
auto risks = detector_->detectCollisions();
// 验证结果
ASSERT_EQ(risks.size(), 1); // 应该检测到一个冲突风险
if (!risks.empty()) {
const auto& risk = risks[0];
EXPECT_EQ(risk.id1, "TEST001"); // 航空器ID
EXPECT_EQ(risk.id2, "VEH001"); // 车辆ID
EXPECT_EQ(risk.distance, 20); // 距离应该是20米
EXPECT_EQ(risk.level, RiskLevel::CRITICAL); // 20米距离应该是严重风险
EXPECT_GT(risk.relativeSpeed, 0); // 相对速度应该大于0
}
}
// 测试可控车辆与其他车辆的冲突检测
TEST_F(CollisionDetectorTest, DetectControllableVehicleOtherVehicleCollision) {
// 设置 Mock 期望
EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH001"))
.WillRepeatedly(testing::Return(true));
EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH002"))
.WillRepeatedly(testing::Return(false));
// 设置测试数<E8AF95><E695B0><EFBFBD>
Vehicle controlVehicle;
controlVehicle.vehicleNo = "VEH001";
controlVehicle.position = {100, 100};
controlVehicle.speed = 5;
controlVehicle.heading = 90;
Vehicle otherVehicle;
otherVehicle.vehicleNo = "VEH002";
otherVehicle.position = {120, 100}; // 距离可控车辆20米
otherVehicle.speed = 5;
otherVehicle.heading = 270;
// 更新交通数据
detector_->updateTraffic({}, {controlVehicle, otherVehicle});
// 执行冲突检测
auto risks = detector_->detectCollisions();
// 验证结果
ASSERT_EQ(risks.size(), 1); // 应该检测到一个冲突风险
if (!risks.empty()) {
EXPECT_EQ(risks[0].id1, "VEH001"); // 可控车辆ID
EXPECT_EQ(risks[0].id2, "VEH002"); // 其他车辆ID
EXPECT_EQ(risks[0].distance, 20); // 距离应该是20米
EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL); // 20米距离应该是告警
}
}
// 测试非可控车辆的冲突检测（不应该产生冲突风险）
TEST_F(CollisionDetectorTest, NonControllableVehicleCollision) {
// 设置测试数据
Vehicle vehicle1;
vehicle1.vehicleNo = "VEH001";
vehicle1.position = {100, 100};
vehicle1.speed = 5;
vehicle1.heading = 90;
Vehicle vehicle2;
vehicle2.vehicleNo = "VEH002";
vehicle2.position = {120, 100}; // 距离20米
vehicle2.speed = 5;
vehicle2.heading = 270;
// 设置 Mock 期望
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
.WillRepeatedly(testing::Return(false));
// 更新交通数据
detector_->updateTraffic({}, {vehicle1, vehicle2});
// 执行冲突检测
auto risks = detector_->detectCollisions();
// 验证结果
EXPECT_EQ(risks.size(), 0); // 非可控车辆之间的冲突不应该被检测
}
// 测试多个可控车辆之间的冲突检测
TEST_F(CollisionDetectorTest, MultipleControllableVehiclesCollision) {
// 设置 Mock 期望 - 所有车辆都是可控的
ON_CALL(*mockControllableVehicles_, isControllable(testing::_))
.WillByDefault(testing::Return(true));
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
.Times(testing::AtLeast(3)); // 至少调用3次，因为有3辆车
Logger::info("Set mock expectation: all vehicles are controllable");
// 设置测试数据
std::vector<Vehicle> vehicles;
// VEH001 在 (100, 100)
Vehicle v1;
v1.vehicleNo = "VEH001";
v1.position = {100.0, 100.0};
v1.speed = 5;
v1.heading = 90;
vehicles.push_back(v1);
// VEH002 在 (120, 100)，与 VEH001 相距 20 米
Vehicle v2;
v2.vehicleNo = "VEH002";
v2.position = {120.0, 100.0};
v2.speed = 5;
v2.heading = 90;
vehicles.push_back(v2);
// VEH003 在 (200, 100)，与其他车辆距离超过阈值
Vehicle v3;
v3.vehicleNo = "VEH003";
v3.position = {200.0, 100.0};
v3.speed = 5;
v3.heading = 90;
vehicles.push_back(v3);
// 更新交通数据
detector_->updateTraffic({}, vehicles);
// 执行冲突检测
auto risks = detector_->detectCollisions();
// 验证结果
EXPECT_EQ(risks.size(), 1); // 应该只检测到1个冲突风险（VEH001和VEH002之间）
if (risks.size() == 1) {
// 验证冲突风险的详细信息
EXPECT_TRUE((risks[0].id1 == "VEH001" && risks[0].id2 == "VEH002") ||
(risks[0].id1 == "VEH002" && risks[0].id2 == "VEH001"));
EXPECT_EQ(risks[0].distance, 20.0);
EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL); // 20米应该是告警级别
}
}
// 性能测试：模拟真实机场场景
TEST_F(CollisionDetectorTest, PerformanceTest) {
// 设置 Mock 期望 - 默认车辆不可控
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
.WillRepeatedly(testing::Return(false));
// 设3辆可控车辆
std::vector<std::string> controlVehicleNos = {
"VEH001", // 滑行道上的可控车辆
"VEH002", // 停机位的可控车辆
"VEH003" // 服务区的可控车辆
};
for (const auto& vehicleNo : controlVehicleNos) {
EXPECT_CALL(*mockControllableVehicles_, isControllable(vehicleNo))
.WillRepeatedly(testing::Return(true));
}
Logger::info("Set mock expectations for controllable vehicles");
// 创建测试数据
std::vector<Aircraft> aircraft;
std::vector<Vehicle> vehicles;
// 跑道区域：5架航空器
for (int i = 0; i < 5; ++i) {
Aircraft a;
a.flightNo = "RW" + std::to_string(i + 1);
a.position = {1800.0 + i * 500, 30.0}; // 跑道上等间距分布
a.speed = 30; // 较快速度
a.heading = 90;
aircraft.push_back(a);
}
// 滑行道：5架航空器
for (int i = 0; i < 5; ++i) {
Aircraft a;
a.flightNo = "TW" + std::to_string(i + 1);
a.position = {1800.0 + i * 500, 90.0}; // 滑行道上等间距分布
a.speed = 10; // 中等速度
a.heading = 90;
aircraft.push_back(a);
}
// 停机位区域：100架航空器，180辆<30><E8BE86><EFBFBD>辆
for (int i = 0; i < 100; ++i) {
Aircraft a;
a.flightNo = "GT" + std::to_string(i + 1);
a.position = {
750.0 + (i % 10) * 150, // 10列
500.0 + (i / 10) * 100 // 10行
};
a.speed = 0; // 静止
a.heading = 180;
aircraft.push_back(a);
}
for (int i = 0; i < 180; ++i) {
Vehicle v;
v.vehicleNo = "GV" + std::to_string(i + 1);
v.position = {
750.0 + (i % 12) * 125, // 12列
500.0 + (i / 12) * 83 // 15行
};
v.speed = 5; // 低速
v.heading = (i % 4) * 90; // 4个方向
vehicles.push_back(v);
}
// 服务区：40架航空器，120辆车辆
for (int i = 0; i < 40; ++i) {
Aircraft a;
a.flightNo = "SA" + std::to_string(i + 1);
a.position = {
1000.0 + (i % 8) * 250, // 8列
500.0 + (i / 8) * 200 // 5行
};
a.speed = 0; // 静止
a.heading = 180;
aircraft.push_back(a);
}
for (int i = 0; i < 120; ++i) {
Vehicle v;
v.vehicleNo = "SV" + std::to_string(i + 1);
v.position = {
1000.0 + (i % 10) * 200, // 10列
500.0 + (i / 10) * 100 // 12行
};
v.speed = 8; // 中等速度
v.heading = (i % 8) * 45; // 8个方向
vehicles.push_back(v);
}
// 添加3辆可控车辆
// 1. 滑行道上的可控车辆
Vehicle taxiwayVehicle;
taxiwayVehicle.vehicleNo = "VEH001";
taxiwayVehicle.position = {1800.0, 90.0}; // 在滑行道上
taxiwayVehicle.speed = 10;
taxiwayVehicle.heading = 90;
vehicles.push_back(taxiwayVehicle);
// 2. 停机位的可控车辆
Vehicle gateVehicle;
gateVehicle.vehicleNo = "VEH002";
gateVehicle.position = {750.0, 500.0}; // 在停机位区域
gateVehicle.speed = 5;
gateVehicle.heading = 180;
vehicles.push_back(gateVehicle);
// 3. 服务区的可控车辆
Vehicle serviceVehicle;
serviceVehicle.vehicleNo = "VEH003";
serviceVehicle.position = {1000.0, 500.0}; // 在服务区
serviceVehicle.speed = 8;
serviceVehicle.heading = 270;
vehicles.push_back(serviceVehicle);
// 更新交通数据
detector_->updateTraffic(aircraft, vehicles);
Logger::info("Updated traffic data with ", aircraft.size(), " aircraft and ",
vehicles.size(), " vehicles (including 3 controllable vehicles)");
// 执行冲突检测并记录时间
auto start = std::chrono::high_resolution_clock::now();
auto risks = detector_->detectCollisions();
auto end = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
Logger::info("Collision detection completed in ", duration.count(), " microseconds");
Logger::info("Found ", risks.size(), " risks");
// 验证结果
ASSERT_GT(risks.size(), 0); // 应该检测到一些冲突风险
// 验证性能要求
EXPECT_LT(duration.count(), 100000); // 期望处理时间小于100ms
}
// 修改矩形碰撞检测测试
TEST_F(CollisionDetectorTest, RectangleCollisionParallelMotion) {
// 设置系统配置
SystemConfig::instance().collision_detection.prediction.vehicle_size = 10.0; // 改为10米全边长
SystemConfig::instance().collision_detection.prediction.aircraft_size = 20.0; // 改为20米全边长
// 创建两个平行运动的车辆
Vehicle v1;
v1.vehicleNo = "VEH001";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.heading = 90.0; // 向东行驶
Vehicle v2;
v2.vehicleNo = "VEH002";
v2.position = {100.0, 107.0}; // 在v1北侧7米处
v2.speed = 10.0;
v2.heading = 90.0; // 向东行驶
// 检测碰撞
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
// 由<><E794B1><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;
v1.vehicleNo = "VEH001";
v1.position = {0.0, 100.0};
v1.speed = 10.0;
v1.heading = 90.0; // 向东行驶
Vehicle v2;
v2.vehicleNo = "VEH002";
v2.position = {100.0, 200.0};
v2.speed = 10.0;
v2.heading = 180.0; // 向南行驶
// 检测碰撞
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
// 两车将在(100, 100)处相遇，应该在30秒时间窗口内检测到碰撞
EXPECT_TRUE(hasCollision) << "交叉路径的车辆应该在30秒时间窗口内检测到碰撞";
}
TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) {
// 设置系统配置
SystemConfig::instance().collision_detection.prediction.vehicle_size = 10.0; // 改为10米全边长
SystemConfig::instance().collision_detection.prediction.aircraft_size = 20.0; // 改为20米全边长
// 创建一个航空器和一个车辆
Aircraft aircraft;
aircraft.flightNo = "TEST001";
aircraft.position = {100.0, 100.0};
aircraft.speed = 15.0;
aircraft.heading = 90.0; // 向东行驶
aircraft.type = MovingObjectType::AIRCRAFT;
Vehicle vehicle;
vehicle.vehicleNo = "VEH001";
vehicle.position = {120.0, 105.0}; // 在航空器前方偏北5米处
vehicle.speed = 8.0;
vehicle.heading = 270.0; // 向西行驶（与航空器相向而行）
vehicle.type = MovingObjectType::UNMANNED;
vehicle.isControllable = true;
// 输出测试配置
std::cout << "\n=== 矩形碰撞检测测试（航空器-车辆） ===" << std::endl;
std::cout << "配置信息：" << std::endl;
std::cout << "- 航空器尺寸: " << SystemConfig::instance().collision_detection.prediction.aircraft_size << "米" << std::endl;
std::cout << "- 车辆尺寸: " << SystemConfig::instance().collision_detection.prediction.vehicle_size << "米" << std::endl;
// 测试1：相向而行且距离较近
auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "相向而行且距离较近的航空器和无人车应该检测到碰撞";
std::cout << "\n航空器信息：" << std::endl;
std::cout << "- 编号: " << aircraft.flightNo << std::endl;
std::cout << "- 位置: (" << aircraft.position.x << ", " << aircraft.position.y << ")" << std::endl;
std::cout << "- 速度: " << aircraft.speed << "米/秒" << std::endl;
std::cout << "- 朝向: " << aircraft.heading << "度" << std::endl;
// 测试2：增加距离到边界值
vehicle.position = {150.0, 105.0}; // 增加到50米距离
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "在警告距离内应该检测到碰撞";
std::cout << "\n车辆信息：" << std::endl;
std::cout << "- 编号: " << vehicle.vehicleNo << std::endl;
std::cout << "- 位置: (" << vehicle.position.x << ", " << vehicle.position.y << ")" << std::endl;
std::cout << "- 速度: " << vehicle.speed << "米/秒" << std::endl;
std::cout << "- 朝向: " << vehicle.heading << "度" << std::endl;
// 计算实际距离
double dx = vehicle.position.x - aircraft.position.x;
double dy = vehicle.position.y - aircraft.position.y;
double distance = std::sqrt(dx*dx + dy*dy);
std::cout << "\n实际距离: " << distance << "米" << std::endl;
// 计算相对速度
double relativeSpeed = aircraft.speed + vehicle.speed; // 相向而行，速度相加
std::cout << "相对速度: " << relativeSpeed << "米/秒" << std::endl;
// 预计碰撞时间
double timeToCollision = distance / relativeSpeed;
std::cout << "预计碰撞时间: " << timeToCollision << "秒" << std::endl;
// 检测碰撞
bool hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0);
std::cout << "\n碰撞检测结果: " << (hasCollision ? "是" : "否") << std::endl;
// 由于距离较近且相向而行，应该在30秒时间窗口内检测到碰撞
EXPECT_TRUE(hasCollision) << "相向而行且距离较近的航空器和车辆应该在30秒时间窗口内检测到碰撞";
// 测试边界情况：增加距离
// 测试3：超出碰撞检测范围
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 = {110.0, 100.0}; // 距离缩短到10米
vehicle.speed = 0.0;
aircraft.speed = 0.0;
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0);
EXPECT_TRUE(hasCollision) << "静止状态下距离较近时应该在30秒时间窗口内检测到碰撞";
// 测试边界情况：垂直路径
vehicle.position = {100.0, 120.0}; // 在航空器正北方
vehicle.speed = 10.0;
vehicle.heading = 180.0; // 向南行驶
aircraft.speed = 10.0;
aircraft.heading = 90.0; // 向东行驶
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0);
EXPECT_TRUE(hasCollision) << "垂直路径且距离较近时应该在30秒时间窗口内检测到碰撞";
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞";
}
TEST_F(CollisionDetectorTest, RectangleCollisionWithRotation) {
// 设置系统配置
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0;
// 测试无人车与特勤车的碰撞检测
TEST_F(CollisionDetectorTest, UnmannedVehicleSpecialVehicleCollision) {
Vehicle unmanned;
unmanned.vehicleNo = "VEH001";
unmanned.position = {100.0, 100.0};
unmanned.speed = 10.0;
unmanned.heading = 90.0; // 向东行驶
unmanned.type = MovingObjectType::UNMANNED;
unmanned.isControllable = true;
// 创建两个有一定角度的车辆
Vehicle special;
special.vehicleNo = "VEH002";
special.position = {120.0, 100.0}; // 距离20米
special.speed = 5.0;
special.heading = 270.0; // 向西行驶
special.type = MovingObjectType::SPECIAL;
special.isControllable = false;
// 测试1：相向而行且距离较近
auto collisionResult = detector_->checkCollision(unmanned, special, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "相向而行且距离较近的无人车和特勤车应该检测到碰撞";
// 测试2：在警告距离边界
special.position = {150.0, 100.0}; // 增加到50米距离
collisionResult = detector_->checkCollision(unmanned, special, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "在警告距离应该检测到碰撞";
// 测试3：超出碰撞检测范围
special.position = {300.0, 100.0}; // 增加到200米距离
collisionResult = detector_->checkCollision(unmanned, special, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "超出警告距离时不应该检测到碰撞";
}
// 测试斜向叉路径碰撞
TEST_F(CollisionDetectorTest, DiagonalCrossingPathsCollision) {
Vehicle v1;
v1.vehicleNo = "VEH001";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.heading = 45.0; // 向东北行驶
v1.heading = 45.0; // 北行驶
v1.type = MovingObjectType::UNMANNED;
v1.isControllable = true;
Vehicle v2;
v2.vehicleNo = "VEH002";
v2.position = {105.0, 105.0}; // 距离缩短到约7.07米
v2.speed = 10.0;
v2.heading = 315.0; // 向西北行驶
// 检测碰撞
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
// 由于两车轨迹交叉且距离较近，应该在30秒时间窗口内检测到碰撞
EXPECT_TRUE(hasCollision) << "不同角度的车辆相交时应该在30秒时间窗口内检测到碰撞";
// 将v2移动到更远的位置
v2.position = {150.0, 150.0};
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
v2.speed = 10.0;
v2.heading = 225.0; // 向西南行驶（与v1相向）
v2.type = MovingObjectType::UNMANNED;
v2.isControllable = true;
// 距离较远时不应该检测到碰撞
EXPECT_FALSE(hasCollision) << "距离较远时不应该在30秒时间窗口内检测到碰撞";
// 测试1：路径交叉且距离较近
auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "斜向相向而行且路径交叉的车辆应该检测到碰撞";
// 测试2：不同速度比
v1.speed = 5.0; // 降低速度
v2.speed = 15.0; // 提高速度
collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "不同速度比的斜向相向车辆应该检测到碰撞";
// 测试3：超出碰撞检测范围
v2.position = {300.0, 300.0}; // 增加到较远距离
collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞";
}
TEST_F(CollisionDetectorTest, RectangleCollisionEdgeCases) {
// 设置系统配置
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0;
// 测试垂直交叉路径碰撞
TEST_F(CollisionDetectorTest, PerpendicularCrossingPathsCollision) {
Vehicle v1;
v1.vehicleNo = "VEH001";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.heading = 90.0; // 向东行驶
v1.type = MovingObjectType::UNMANNED;
v1.isControllable = true;
Vehicle v2;
v2.vehicleNo = "VEH002";
v2.position = {120.0, 120.0}; // 在v1东北方20米处
v2.speed = 10.0;
v2.heading = 180.0; // 向南行驶
v2.type = MovingObjectType::UNMANNED;
v2.isControllable = true;
// 测试1：垂直相交且距离较近
auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "垂直交叉路径的车辆应该检测到碰撞";
// 测试2：不同速度比
v1.speed = 5.0; // 降低速度
v2.speed = 15.0; // 提高速度
collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "不同速度的垂直交叉路径车辆应该检测到碰撞";
// 测试3：超出碰撞检测范围
v2.position = {200.0, 200.0}; // 增加到较远距离
collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "距离较远时应该检测到碰撞";
}
// 测试静止辆的碰撞检测
TEST_F(CollisionDetectorTest, StationaryVehiclesCollision) {
Vehicle v1;
v1.vehicleNo = "VEH001";
v1.position = {100.0, 100.0};
v1.speed = 0.0; // 静止
v1.heading = 90.0;
v1.type = MovingObjectType::UNMANNED;
v1.isControllable = true;
Vehicle v2;
v2.vehicleNo = "VEH002";
// 测试1：两个静止车辆，刚好接触
v2.position = {105.0, 100.0}; // 距离为5米（两车长度和的一半）
v2.speed = 0.0;
v2.position = {105.0, 100.0}; // 距离5米
v2.speed = 0.0; // 静止
v2.heading = 90.0;
v2.type = MovingObjectType::UNMANNED;
v2.isControllable = true;
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
EXPECT_TRUE(hasCollision) << "两个静止且刚好接触的车辆应该在30秒时间窗口内检测到碰撞";
// 测试1：两个静止车辆距离小于安全距离
auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "距离小于安全距离的静止车辆应该检测到碰撞";
// 测试2：两个静止车辆，距离略大于安全距离
v2.position = {111.0, 100.0}; // 距离为11米
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
EXPECT_FALSE(hasCollision) << "两个静止且距离大于安全距离的车辆不应该在30秒时间窗口内检测到碰撞";
// 测试2：一动一静，移动车辆接近静止车辆
v2.position = {120.0, 100.0}; // 增加距离到20米
v2.speed = 5.0; // 开始移动
v2.heading = 270.0; // 向西行驶（朝向v1）
collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "移动车辆接近静止车辆时应该检测到碰撞";
// 测试3：极低速度
v2.position = {120.0, 100.0};
v2.speed = 0.1; // 极低速度
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0);
EXPECT_FALSE(hasCollision) << "距离足够且速度极低的车辆不应该在30秒时间窗口内检测到碰撞";
// 测试3：一动一静，移动车辆远离静止车辆
v2.position = {160.0, 100.0}; // 增加到60米，大于安全距离(50米)
v2.speed = 5.0; // 开始移动
v2.heading = 90.0; // 向东行驶（远离v1）
collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "移动车辆远离静止车辆且距离<EFBFBD><EFBFBD><EFBFBD>于安全距离时不应该检测到碰撞";
}
// 测试同向运动的碰撞检测
TEST_F(CollisionDetectorTest, TailgatingCollision) {
Vehicle v1;
v1.vehicleNo = "VEH001";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.heading = 90.0; // 向东行驶
v1.type = MovingObjectType::UNMANNED;
v1.isControllable = true;
Vehicle v2;
v2.vehicleNo = "VEH002";
v2.position = {40.0, 100.0}; // 在v1西侧60米处（100-60=40）
v2.speed = 10.0;
v2.heading = 90.0; // 也向东行驶
v2.type = MovingObjectType::UNMANNED;
v2.isControllable = true;
// 测试1：同向同速
auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "同向同速的车辆不应该检测到碰撞";
// 测试2：同向不同速（v2速度更快，会追上v1）
v2.speed = 15.0;
Logger::debug(
"同向追尾测试: ",
"v1=(", v1.position.x, ",", v1.position.y, "), speed=", v1.speed,
", v2=(", v2.position.x, ",", v2.position.y, "), speed=", v2.speed,
", 相对速度=", v2.speed - v1.speed,
", 初始距离=", std::sqrt(
std::pow(v2.position.x - v1.position.x, 2) +
std::pow(v2.position.y - v1.position.y, 2)
)
);
collisionResult = detector_->checkCollision(v1, v2, 30.0);
Logger::debug(
"碰撞检测结果: willCollide=", collisionResult.willCollide,
", timeToCollision=", collisionResult.timeToCollision,
", minDistance=", collisionResult.minDistance,
", type=", static_cast<int>(collisionResult.type)
);
EXPECT_TRUE(collisionResult.willCollide) << "同向但速度较快的车辆追上前车时应该检测到碰撞";
// 测试3：同向不同速但距离较远
v2.position = {0.0, 100.0}; // 增加到100米距离
collisionResult = detector_->checkCollision(v1, v2, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞";
}
// 测试航空器与静止车辆的碰撞检测
TEST_F(CollisionDetectorTest, AircraftStationaryVehicleCollision) {
Aircraft aircraft;
aircraft.flightNo = "TEST001";
aircraft.position = {100.0, 100.0};
aircraft.speed = 15.0;
aircraft.heading = 90.0; // 向东行驶
aircraft.type = MovingObjectType::AIRCRAFT;
Vehicle vehicle;
vehicle.vehicleNo = "VEH001";
vehicle.position = {150.0, 100.0}; // 在航空器前方50米处
vehicle.speed = 0.0; // 静止
vehicle.heading = 90.0;
vehicle.type = MovingObjectType::UNMANNED;
vehicle.isControllable = true;
// 测试1：航空器接近静止车辆
auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "航空器接近静止车辆时应该检测到碰撞";
// 测试2：静止车辆在航空器航向偏离处
vehicle.position = {200.0, 170.0}; // 在航空器前方偏北，距离约100米（大于安全距离75米）
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "航空器与不在航向上的静止车辆不应该检测到碰撞";
// 测试3：静止车辆在航空器后方
vehicle.position = {0.0, 100.0}; // 在航空器后方100米（大于安全距离75米）
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "航空器与后方的静止车辆不应该检测到碰撞";
}