512 lines
21 KiB
C++
512 lines
21 KiB
C++
#include "CollisionDetector.h"
|
||
|
||
#include "config/SystemConfig.h"
|
||
#include "utils/Logger.h"
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <unordered_set>
|
||
|
||
CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles)
|
||
: airportBounds_(bounds),
|
||
vehicleTree_(bounds.getAirportBounds(), 8), // 使用机场总边界初始化四叉树
|
||
controllableVehicles_(&controllableVehicles) {
|
||
// 记录初始化信息
|
||
const auto& airportBounds = bounds.getAirportBounds();
|
||
}
|
||
|
||
void CollisionDetector::updateTraffic(const std::vector<Aircraft>& aircraft,
|
||
const std::vector<Vehicle>& vehicles) {
|
||
Logger::debug("更新交通数据: 航空器数量=", aircraft.size(),
|
||
", 车辆数量=", vehicles.size());
|
||
|
||
// 更新航空器数据
|
||
aircraftData_ = aircraft;
|
||
|
||
// 更新车辆四叉树
|
||
vehicleTree_.clear();
|
||
size_t validVehicles = 0;
|
||
|
||
for (const auto& vehicle : vehicles) {
|
||
// 首先检查车辆是否在机场边界内
|
||
if (!airportBounds_.isPointInBounds(vehicle.position)) {
|
||
Logger::error("车辆在机场边界外: id=", vehicle.id, ", position=(",
|
||
vehicle.position.x, ",", vehicle.position.y, ")");
|
||
continue; // 跳过边界外的车辆
|
||
}
|
||
|
||
// 根据运行时注册表设置车辆类型
|
||
Vehicle updatedVehicle = vehicle;
|
||
bool controllable = controllableVehicles_ && controllableVehicles_->isControllable(vehicle.vehicleNo);
|
||
updatedVehicle.type = controllable ? MovingObjectType::UNMANNED : MovingObjectType::SPECIAL;
|
||
updatedVehicle.isControllable = controllable;
|
||
|
||
// 插入四叉树
|
||
try {
|
||
vehicleTree_.insert(updatedVehicle);
|
||
++validVehicles;
|
||
} catch (const std::exception& e) {
|
||
Logger::error("无法插入车辆到四叉树: id=", updatedVehicle.id,
|
||
", error=", e.what());
|
||
}
|
||
}
|
||
|
||
// 记录更新后的状态
|
||
auto bounds = vehicleTree_.getBounds();
|
||
Logger::debug("四叉树边界: min=(", bounds.x, ",", bounds.y, "), max=(",
|
||
bounds.x + bounds.width, ",", bounds.y + bounds.height, ")");
|
||
auto allVehicles = vehicleTree_.queryRange(bounds);
|
||
Logger::debug("交通数据更新完成: 有效车辆数量=", validVehicles,
|
||
", 四叉树中的车辆数量=", allVehicles.size());
|
||
}
|
||
|
||
std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||
std::vector<CollisionRisk> risks;
|
||
|
||
// 获取配置参数
|
||
const auto& predictionConfig =
|
||
SystemConfig::instance().collision_detection.prediction;
|
||
|
||
// 获取所有车辆
|
||
auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds());
|
||
|
||
// 前端注册表内的受管车辆（WUREN + TEQIN）
|
||
std::vector<Vehicle> managedVehicles;
|
||
managedVehicles.reserve(allVehicles.size());
|
||
for (const auto& vehicle : allVehicles) {
|
||
if (controllableVehicles_) {
|
||
if (controllableVehicles_->isManagedVehicle(vehicle.vehicleNo)) {
|
||
managedVehicles.push_back(vehicle);
|
||
}
|
||
} else {
|
||
// 兼容：若未提供注册表，则仅按可控标记处理
|
||
if (vehicle.isControllable) {
|
||
managedVehicles.push_back(vehicle);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 记录当前检测到的冲突对
|
||
std::unordered_set<std::pair<std::string, std::string>, CollisionPairHash>
|
||
currentCollisions;
|
||
|
||
// 检测受管车辆与航空器的碰撞
|
||
for (const auto& aircraft : aircraftData_) {
|
||
for (const auto& vehicle : managedVehicles) {
|
||
// 获取冲突记录键
|
||
auto collisionKey = getCollisionKey(aircraft.id, vehicle.id);
|
||
|
||
// 获取区域配置
|
||
const auto& areaConfig = getCollisionParams(aircraft.position);
|
||
double safeDistance = areaConfig.getThresholds(aircraft.type, vehicle.type).warning;
|
||
|
||
// 检查是否存在未解除的冲突记录
|
||
auto it = collisionRecords_.find(collisionKey);
|
||
bool hasUnresolvedConflict =
|
||
(it != collisionRecords_.end() && !it->second.resolved);
|
||
|
||
if (hasUnresolvedConflict) {
|
||
Logger::debug("存在未解除的冲突记录: obj1=", aircraft.id,
|
||
", obj2=", vehicle.id, ", maxLevel=",
|
||
static_cast<int>(it->second.maxLevel),
|
||
", collisionPoint=(", it->second.collisionPoint.x,
|
||
",", it->second.collisionPoint.y, ")");
|
||
}
|
||
|
||
// 检测碰撞
|
||
auto collisionResult =
|
||
checkCollision(aircraft, vehicle, predictionConfig.time_window);
|
||
|
||
// 计算相对运动
|
||
MovementVector av(aircraft.speed, aircraft.heading);
|
||
MovementVector vv(vehicle.speed, vehicle.heading);
|
||
double vx = av.vx - vv.vx;
|
||
double vy = av.vy - vv.vy;
|
||
double relativeSpeed = std::sqrt(vx * vx + vy * vy);
|
||
|
||
// 评估风险等级
|
||
auto [level, zoneType] =
|
||
evaluateRisk(collisionResult, aircraft.position, aircraft.type,
|
||
vehicle.type);
|
||
|
||
if (level != RiskLevel::NONE) {
|
||
// 记录或更新冲突信息
|
||
auto& record = collisionRecords_[collisionKey];
|
||
if (!record.resolved) {
|
||
// 更新已存在的冲突记录
|
||
record.maxLevel = std::max(record.maxLevel, level);
|
||
record.collisionPoint =
|
||
collisionResult.collisionPoint; // 更新冲突点
|
||
Logger::debug("更新冲突记录: obj1=", aircraft.id,
|
||
", obj2=", vehicle.id, ", maxLevel=",
|
||
static_cast<int>(record.maxLevel),
|
||
", collisionPoint=(", record.collisionPoint.x,
|
||
",", record.collisionPoint.y, ")");
|
||
} else {
|
||
// 创建新的冲突记录
|
||
record = { collisionResult
|
||
.collisionPoint, // 使用当前检测到的冲突点
|
||
std::chrono::system_clock::now()
|
||
.time_since_epoch()
|
||
.count(),
|
||
level, false };
|
||
Logger::debug("创建新的冲突记录: obj1=", aircraft.id,
|
||
", obj2=", vehicle.id,
|
||
", level=", static_cast<int>(level),
|
||
", collisionPoint=(",
|
||
collisionResult.collisionPoint.x, ",",
|
||
collisionResult.collisionPoint.y, ")");
|
||
}
|
||
|
||
// 添加到当前冲突集合
|
||
currentCollisions.insert(collisionKey);
|
||
|
||
// 添加到风险列表
|
||
risks.push_back({ aircraft.id,
|
||
vehicle.id,
|
||
level,
|
||
collisionResult.distance,
|
||
collisionResult.minDistance,
|
||
relativeSpeed,
|
||
{vx, vy},
|
||
zoneType,
|
||
collisionResult.timeToCollision,
|
||
collisionResult.collisionPoint });
|
||
|
||
Logger::debug(
|
||
"检测到碰撞风险: obj1=", aircraft.id, ", obj2=", vehicle.id,
|
||
", minDistance=", collisionResult.minDistance, "m",
|
||
", timeToCollision=", collisionResult.timeToCollision, "s",
|
||
", level=", static_cast<int>(level));
|
||
} else if (hasUnresolvedConflict) {
|
||
// 检查是否满足解除条件
|
||
if (checkCollisionResolved(aircraft, vehicle, it->second,
|
||
safeDistance)) {
|
||
it->second.resolved = true;
|
||
Logger::debug("冲突已解除: obj1=", aircraft.id,
|
||
", obj2=", vehicle.id);
|
||
} else {
|
||
// 虽然当前没有检测到风险，但由于未满足解除条件，继续保持原有风险等级
|
||
currentCollisions.insert(collisionKey);
|
||
risks.push_back({ aircraft.id,
|
||
vehicle.id,
|
||
it->second.maxLevel,
|
||
collisionResult.distance,
|
||
collisionResult.minDistance,
|
||
relativeSpeed,
|
||
{vx, vy},
|
||
zoneType,
|
||
collisionResult.timeToCollision,
|
||
it->second.collisionPoint });
|
||
Logger::debug(
|
||
"保持原有风险等级: obj1=", aircraft.id,
|
||
", obj2=", vehicle.id,
|
||
", maxLevel=", static_cast<int>(it->second.maxLevel),
|
||
", minDistance=", collisionResult.minDistance, "m",
|
||
", timeToCollision=", collisionResult.timeToCollision,
|
||
"s");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 清理已解除的冲突记录
|
||
for (auto it = collisionRecords_.begin(); it != collisionRecords_.end();) {
|
||
if (it->second.resolved) {
|
||
it = collisionRecords_.erase(it);
|
||
} else {
|
||
++it;
|
||
}
|
||
}
|
||
|
||
return risks;
|
||
}
|
||
|
||
bool CollisionDetector::checkCollisionResolved(const MovingObject& obj1,
|
||
const MovingObject& obj2,
|
||
const CollisionRecord& record,
|
||
double safeDistance) const {
|
||
// 计算航空器与冲突点的距离
|
||
double dist1 =
|
||
std::sqrt(std::pow(obj1.position.x - record.collisionPoint.x, 2) +
|
||
std::pow(obj1.position.y - record.collisionPoint.y, 2));
|
||
|
||
double dist2 =
|
||
std::sqrt(std::pow(obj2.position.x - record.collisionPoint.x, 2) +
|
||
std::pow(obj2.position.y - record.collisionPoint.y, 2));
|
||
|
||
// 计算冲突点相对于航空器的位置向量与航空器航向的夹角
|
||
double dx =
|
||
record.collisionPoint.x - obj1.position.x; // 冲突点相对于航空器的向量
|
||
double dy = record.collisionPoint.y - obj1.position.y;
|
||
double pointAngle =
|
||
std::atan2(dy, dx) * 180.0 / M_PI; // 冲突点相对于航空器的角度
|
||
double angleDiff =
|
||
normalizeAngle(pointAngle - (90.0 - obj1.heading)); // 修正航向角
|
||
|
||
Logger::debug("检查冲突解除条件: 航空器位置=(", obj1.position.x, ",",
|
||
obj1.position.y, "), 冲突点=(", record.collisionPoint.x, ",",
|
||
record.collisionPoint.y, "), 航向=", obj1.heading,
|
||
", 冲突点角度=", pointAngle, ", 角度差=", angleDiff,
|
||
", 距离=", dist1);
|
||
|
||
// 判断航空器是否已经通过交叉点
|
||
if (angleDiff > 90 && angleDiff < 270) {
|
||
// 航空器已经通过交叉点，只要航空器离开冲突点的距离大于安全距离就可以解除
|
||
Logger::debug("航空器已通过交叉点，航空器距离=", dist1,
|
||
"m, 安全距离=", safeDistance, "m");
|
||
return dist1 >= safeDistance;
|
||
} else {
|
||
// 航空器还未通过交叉点，需要两者都在安全距离外且预计不会碰撞
|
||
auto collisionResult =
|
||
checkCollision(obj1, obj2, 30.0); // 使用30秒的预测窗口
|
||
bool willCollide = collisionResult.willCollide;
|
||
bool bothOutside = (dist1 >= safeDistance && dist2 >= safeDistance);
|
||
|
||
Logger::debug("航空器未通过交叉点，航空器距离=", dist1,
|
||
"m, 无人车距离=", dist2, "m, 安全距离=", safeDistance,
|
||
"m, 预测碰撞=", willCollide ? "是" : "否");
|
||
|
||
return !willCollide && bothOutside;
|
||
}
|
||
}
|
||
|
||
// 统一的风险评估函数
|
||
std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
|
||
const CollisionResult& collisionResult, const Vector2D& position,
|
||
MovingObjectType type1, MovingObjectType type2) const {
|
||
|
||
// 取区域配置
|
||
const auto& areaConfig = getCollisionParams(position);
|
||
auto thresholds = areaConfig.getThresholds(type1, type2);
|
||
|
||
RiskLevel level = RiskLevel::NONE;
|
||
WarningZoneType zoneType = WarningZoneType::NONE;
|
||
|
||
// 如果预测到碰撞，根据当前距离判断风险等级
|
||
if (collisionResult.willCollide) {
|
||
// 当前距离小于警报距离 -> 危险等级
|
||
if (collisionResult.distance <= thresholds.critical) {
|
||
level = RiskLevel::CRITICAL;
|
||
zoneType = WarningZoneType::DANGER;
|
||
}
|
||
// 当前距离在预警范围内 -> 预警等级
|
||
else if (collisionResult.distance <= thresholds.warning) {
|
||
level = RiskLevel::WARNING;
|
||
zoneType = WarningZoneType::WARNING;
|
||
}
|
||
}
|
||
|
||
// 修改日志，添加物体类型信息
|
||
Logger::debug("风险评估: 物体1类型=", static_cast<int>(type1),
|
||
", 物体2类型=", static_cast<int>(type2),
|
||
", 当前距离=", collisionResult.distance,
|
||
"m, 预测最小距离=", collisionResult.minDistance,
|
||
"m, 预警阈值=", thresholds.warning,
|
||
"m, 警报阈值=", thresholds.critical,
|
||
"m, 预测碰撞=", collisionResult.willCollide ? "是" : "否",
|
||
", 风险等级=", static_cast<int>(level),
|
||
", 区域类型=", static_cast<int>(zoneType));
|
||
|
||
return { level, zoneType };
|
||
}
|
||
|
||
// 统一的碰撞检测函数
|
||
CollisionResult CollisionDetector::checkCollision(const MovingObject& obj1,
|
||
const MovingObject& obj2,
|
||
double timeWindow) const {
|
||
|
||
// 添加物体信息日志
|
||
Logger::debug("检查碰撞: obj1[", obj1.id, "]=(", obj1.position.x, ",",
|
||
obj1.position.y, "), obj2[", obj2.id, "]=(", obj2.position.x,
|
||
",", obj2.position.y, ")");
|
||
|
||
// 获取区域配置
|
||
const auto& areaConfig = getCollisionParams(obj1.position);
|
||
|
||
// 获取各自的碰撞半径和警告半径
|
||
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);
|
||
|
||
// 计算当前距离
|
||
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 (areaConfig.height_threshold > 0.0) {
|
||
if (obj1.isAircraft()) {
|
||
const auto& ac = static_cast<const Aircraft&>(obj1);
|
||
if (std::isfinite(ac.altitude) && ac.altitude > areaConfig.height_threshold) {
|
||
CollisionResult result;
|
||
result.willCollide = false;
|
||
result.minDistance = current_distance;
|
||
result.distance = current_distance;
|
||
result.timeToMinDistance = 0.0;
|
||
return result;
|
||
}
|
||
}
|
||
if (obj2.isAircraft()) {
|
||
const auto& ac = static_cast<const Aircraft&>(obj2);
|
||
if (std::isfinite(ac.altitude) && ac.altitude > areaConfig.height_threshold) {
|
||
CollisionResult result;
|
||
result.willCollide = false;
|
||
result.minDistance = current_distance;
|
||
result.distance = current_distance;
|
||
result.timeToMinDistance = 0.0;
|
||
return result;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 如果是静止的无人车，使用基础速度
|
||
double speed1_calc = obj1.speed;
|
||
double speed2_calc = obj2.speed;
|
||
|
||
if (obj1.type == MovingObjectType::UNMANNED && speed1_calc < 0.1) {
|
||
speed1_calc = SystemConfig::instance()
|
||
.collision_detection.prediction
|
||
.min_unmanned_speed; // 使用配置的最低速度
|
||
}
|
||
if (obj2.type == MovingObjectType::UNMANNED && speed2_calc < 0.1) {
|
||
speed2_calc = SystemConfig::instance()
|
||
.collision_detection.prediction
|
||
.min_unmanned_speed; // 使用配置的最低速度
|
||
}
|
||
|
||
// 计算速度分量
|
||
double vx1 =
|
||
speed1_calc * std::cos((90.0 - obj1.heading) * M_PI / 180.0); // x 方向
|
||
double vy1 =
|
||
speed1_calc * std::sin((90.0 - obj1.heading) * M_PI / 180.0); // y 方向
|
||
double vx2 = speed2_calc * std::cos((90.0 - obj2.heading) * M_PI / 180.0);
|
||
double vy2 = speed2_calc * std::sin((90.0 - obj2.heading) * M_PI / 180.0);
|
||
|
||
// 如果当前距离大于警告区域，直接返回不会碰撞
|
||
if (current_distance > (warning_radius1 + warning_radius2)) {
|
||
CollisionResult result;
|
||
result.willCollide = false;
|
||
result.minDistance = current_distance;
|
||
result.distance = current_distance;
|
||
return result;
|
||
}
|
||
|
||
// 否则进行碰撞检测
|
||
return predictCircleBasedCollision(obj1.position, radius1, speed1_calc,
|
||
obj1.heading, obj2.position, radius2,
|
||
speed2_calc, obj2.heading, timeWindow);
|
||
}
|
||
|
||
CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||
const Vector2D& pos1, double radius1, double speed1, double heading1,
|
||
const Vector2D& pos2, double radius2, double speed2, double heading2,
|
||
double timeWindow) const {
|
||
|
||
CollisionResult result;
|
||
|
||
const double safe_distance = radius1 + radius2;
|
||
const double dx = pos2.x - pos1.x;
|
||
const double dy = pos2.y - pos1.y;
|
||
const double current_distance = std::sqrt(dx * dx + dy * dy);
|
||
|
||
result.distance = current_distance;
|
||
result.minDistance = current_distance;
|
||
result.timeToMinDistance = 0.0;
|
||
|
||
// 碰撞类型仅用于标记，不影响连续时间的圆形碰撞求解
|
||
if (speed1 < 0.1 && speed2 < 0.1) {
|
||
result.type = CollisionType::STATIC;
|
||
} else {
|
||
double angle_diff = getAngleDifference(heading1, heading2);
|
||
if (angle_diff > 150.0) {
|
||
result.type = CollisionType::HEAD_ON;
|
||
} else if (angle_diff > 15.0 && angle_diff < 165.0) {
|
||
result.type = CollisionType::CROSSING;
|
||
} else {
|
||
result.type = CollisionType::PARALLEL;
|
||
}
|
||
}
|
||
|
||
// 当前已经重叠
|
||
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 = CollisionObjectState(pos1, speed1, heading1);
|
||
result.object2State = CollisionObjectState(pos2, speed2, heading2);
|
||
return result;
|
||
}
|
||
|
||
// 速度分量（航向：正北0度，顺时针增加；x=东，y=北）
|
||
const double h1 = heading1 * M_PI / 180.0;
|
||
const double h2 = heading2 * M_PI / 180.0;
|
||
const Vector2D v1{ speed1 * std::sin(h1), speed1 * std::cos(h1) };
|
||
const Vector2D v2{ speed2 * std::sin(h2), speed2 * std::cos(h2) };
|
||
const Vector2D r0{ dx, dy };
|
||
const Vector2D v{ v2.x - v1.x, v2.y - v1.y };
|
||
|
||
const double a = v.x * v.x + v.y * v.y;
|
||
const double rv = r0.x * v.x + r0.y * v.y;
|
||
|
||
// 相对速度近似为0：距离不会变化
|
||
if (a < 1e-9) {
|
||
result.object1State = CollisionObjectState(pos1, speed1, heading1);
|
||
result.object2State = CollisionObjectState(pos2, speed2, heading2);
|
||
return result;
|
||
}
|
||
|
||
// 计算时间窗口内的最小距离
|
||
double t_min = -rv / a;
|
||
if (t_min < 0.0) t_min = 0.0;
|
||
if (t_min > timeWindow) t_min = timeWindow;
|
||
const Vector2D r_min{ r0.x + v.x * t_min, r0.y + v.y * t_min };
|
||
const double d_min = std::sqrt(r_min.x * r_min.x + r_min.y * r_min.y);
|
||
result.minDistance = d_min;
|
||
result.timeToMinDistance = t_min;
|
||
result.object1State = CollisionObjectState({ pos1.x + v1.x * t_min, pos1.y + v1.y * t_min }, speed1, heading1);
|
||
result.object2State = CollisionObjectState({ pos2.x + v2.x * t_min, pos2.y + v2.y * t_min }, speed2, heading2);
|
||
|
||
// 计算首次进入安全距离的时间（连续时间解析解）
|
||
// |r0 + v t|^2 = safe^2 => a t^2 + 2(rv) t + (|r0|^2 - safe^2)=0
|
||
const double b = 2.0 * rv;
|
||
const double c = (r0.x * r0.x + r0.y * r0.y) - safe_distance * safe_distance;
|
||
const double disc = b * b - 4.0 * a * c;
|
||
if (disc < 0.0) {
|
||
return result;
|
||
}
|
||
|
||
const double sqrt_disc = std::sqrt(disc);
|
||
const double inv_2a = 1.0 / (2.0 * a);
|
||
double t_hit = (-b - sqrt_disc) * inv_2a;
|
||
double t_exit = (-b + sqrt_disc) * inv_2a;
|
||
|
||
if (t_exit < 0.0) {
|
||
return result;
|
||
}
|
||
if (t_hit < 0.0) {
|
||
t_hit = 0.0;
|
||
}
|
||
if (t_hit > timeWindow) {
|
||
return result;
|
||
}
|
||
|
||
result.willCollide = true;
|
||
result.timeToCollision = t_hit;
|
||
result.timeToMinDistance = std::min(result.timeToMinDistance, t_hit);
|
||
result.minDistance = std::min(result.minDistance, safe_distance);
|
||
|
||
const Vector2D p1_hit{ pos1.x + v1.x * t_hit, pos1.y + v1.y * t_hit };
|
||
const Vector2D p2_hit{ pos2.x + v2.x * t_hit, pos2.y + v2.y * t_hit };
|
||
result.collisionPoint = { (p1_hit.x + p2_hit.x) / 2.0,
|
||
(p1_hit.y + p2_hit.y) / 2.0 };
|
||
result.object1State = CollisionObjectState(p1_hit, speed1, heading1);
|
||
result.object2State = CollisionObjectState(p2_hit, speed2, heading2);
|
||
|
||
return result;
|
||
} |