diff --git a/.cursorrules b/.cursorrules index 912e5af..2c17181 100644 --- a/.cursorrules +++ b/.cursorrules @@ -1,3 +1,17 @@ +请遵循： +- 最小化修改代码，不要修改任何与解决当前问题无关的代码（包括注释），一定不能删除任何其他的代码 +- 使用 C++20 标准 +- 使用 CMake 3.14 及以上版本 +- 使用 nlohmann_json 3.11.3 版本 +- 使用 FetchContent 管理第三方库 +- 使用 Google Test 进行单元测试 +- 使用 Google Mock 进行单元测试 +- 使用 Google Benchmark 进行性能测试 +- 在头文件中定义函数，在源文件中实现函数 +- 在头文件中定义类，在源文件中实现类 +- 使用源代码中的格式化方式和注释风格 +- 日志的字符串格式采用拼接方式，不要使用{}占位符 + 请遵循以下 Markdown 规范： 1. 标题层级 diff --git a/CMakeLists.txt b/CMakeLists.txt index b5e7676..9be3ac1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/config/airport_bounds.json b/config/airport_bounds.json index 2d13917..d0e93b7 100644 --- a/config/airport_bounds.json +++ b/config/airport_bounds.json @@ -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 + } } } } diff --git a/src/config/AirportBounds.h b/src/config/AirportBounds.h index 5e40038..9161cc0 100644 --- a/src/config/AirportBounds.h +++ b/src/config/AirportBounds.h @@ -2,47 +2,10 @@ #define AIRPORT_SPATIAL_AIRPORT_BOUNDS_H #include "spatial/QuadTree.h" +#include "AreaConfig.h" #include #include -// 区域类型 -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: diff --git a/src/config/AreaConfig.h b/src/config/AreaConfig.h new file mode 100644 index 0000000..373fae3 --- /dev/null +++ b/src/config/AreaConfig.h @@ -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 \ No newline at end of file diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index 82d1011..044ab94 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -34,7 +34,18 @@ void CollisionDetector::updateTraffic(const std::vector& 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 CollisionDetector::detectCollisions() { // 获取所有车辆 auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds()); - // 可控车辆 - std::vector controlVehicles; + // 可控车辆（无人车） + std::vector 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 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(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(level), ", zone=", static_cast(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 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(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(level), ", zone=", static_cast(zoneType), - ", relativeSpeed=", relativeSpeed, "m/s, relativeMotion=(", vx, ",", vy, ")", - ", isMovingAway=", relativeSpeed > 0 ? "是" : "否"); + ", relativeSpeed=", relativeSpeed, "m/s", + ", relativeMotion=(", vx, ",", vy, ")"); } } } @@ -203,38 +213,40 @@ std::vector CollisionDetector::detectCollisions() { return risks; } -// 添加一个新的辅助函数来统一处理风险等级和预警区域的判断 +// 统一的风险评估函数 std::pair 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(level), ", 区域类型=", static_cast(zoneType) ); @@ -242,50 +254,406 @@ std::pair 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; } \ No newline at end of file diff --git a/src/detector/CollisionDetector.h b/src/detector/CollisionDetector.h index 8f2caf1..ef9a2b2 100644 --- a/src/detector/CollisionDetector.h +++ b/src/detector/CollisionDetector.h @@ -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 @@ -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 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 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 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 \ No newline at end of file diff --git a/src/detector/CollisionTypes.h b/src/detector/CollisionTypes.h index dc403cb..d41a0b3 100644 --- a/src/detector/CollisionTypes.h +++ b/src/detector/CollisionTypes.h @@ -4,6 +4,7 @@ #define AIRPORT_DETECTOR_COLLISION_TYPES_H #include "types/BasicTypes.h" +#include 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::infinity()), collisionPoint({0, 0}), minDistance(std::numeric_limits::infinity()), + timeToMinDistance(std::numeric_limits::infinity()), type(CollisionType::STATIC) {} }; diff --git a/src/spatial/CoordinateConverter.h b/src/spatial/CoordinateConverter.h index 2930c3b..80cd164 100644 --- a/src/spatial/CoordinateConverter.h +++ b/src/spatial/CoordinateConverter.h @@ -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 #include diff --git a/src/types/BasicTypes.h b/src/types/BasicTypes.h index 4f282d3..37a94b3 100644 --- a/src/types/BasicTypes.h +++ b/src/types/BasicTypes.h @@ -4,6 +4,13 @@ #include #include +// 移动物体类型 +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 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; diff --git a/tests/BasicCollisionTest.cpp b/tests/BasicCollisionTest.cpp new file mode 100644 index 0000000..e4ae782 --- /dev/null +++ b/tests/BasicCollisionTest.cpp @@ -0,0 +1,306 @@ +#include "detector/CollisionDetector.h" +#include "vehicle/ControllableVehicles.h" +#include "config/AirportBounds.h" +#include +#include +#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(); + mockControllableVehicles_ = std::make_unique(); + + // 设置 Mock 对象的行为 + EXPECT_CALL(*airportBounds_, getAreaType(::testing::_)) + .WillRepeatedly(::testing::Return(AreaType::TEST_ZONE)); + + detector_ = std::make_unique(*airportBounds_, *mockControllableVehicles_); + } + + std::unique_ptr airportBounds_; + std::unique_ptr mockControllableVehicles_; + std::unique_ptr 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) << "应该识别为平行��动"; + + // 增加更多验证 + EXPECT_DOUBLE_EQ(result.minDistance, 50.0) << "最小距离应该是初始距离50米"; + EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "平行运动的最小距离时间应该为0"; + EXPECT_DOUBLE_EQ(result.timeToCollision, std::numeric_limits::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; // ��右运动 + 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); +} \ No newline at end of file diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ff8e4e8..f25529d 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,5 +1,6 @@ # 设置测试源文件 set(TEST_SOURCES + BasicCollisionTest.cpp CollisionDetectorTest.cpp BasicTypesTest.cpp HTTPDataSourceTest.cpp diff --git a/tests/CollisionDetectorTest.cpp b/tests/CollisionDetectorTest.cpp index ee84b08..7ff262c 100644 --- a/tests/CollisionDetectorTest.cpp +++ b/tests/CollisionDetectorTest.cpp @@ -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(); mockControllableVehicles_ = std::make_unique(); + // 设置 Mock 对象的行为 + EXPECT_CALL(*airportBounds_, getAreaType(::testing::_)) + .WillRepeatedly(::testing::Return(AreaType::TEST_ZONE)); + // 创建冲突检测器 detector_ = std::make_unique(*airportBounds_, *mockControllableVehicles_); } @@ -85,509 +95,254 @@ protected: std::unique_ptr 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米 - 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)); - - // 设置测试数��� - 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 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 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; - std::vector 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辆���辆 - 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(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); - - // 由���两车间距小于安全距离（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) << "移动车辆远离静止车辆且距离���于安全距离时不应该检测到碰撞"; +} + +// 测试同向运动的碰撞检测 +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(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) << "航空器与后方的静止车辆不应该检测到碰撞"; } \ No newline at end of file