124 lines
4.4 KiB
C++
124 lines
4.4 KiB
C++
#ifndef AIRPORT_DETECTOR_COLLISION_DETECTOR_H
|
||
#define AIRPORT_DETECTOR_COLLISION_DETECTOR_H
|
||
|
||
#include "types/BasicTypes.h"
|
||
#include "spatial/QuadTree.h"
|
||
#include "spatial/AirportBounds.h"
|
||
#include "vehicle/ControllableVehicles.h"
|
||
#include "config/SystemConfig.h"
|
||
#include <vector>
|
||
#include <utility>
|
||
#include <limits>
|
||
#include <utils/Logger.h>
|
||
|
||
// 碰撞风险等级
|
||
enum class RiskLevel {
|
||
NONE = 0, // 无风险
|
||
WARNING = 1, // 预警：进入预警区域
|
||
CRITICAL = 2, // 告警：进入危险区域
|
||
};
|
||
|
||
// 预警区域类型
|
||
enum class WarningZoneType {
|
||
NONE = 0, // 无风险区域
|
||
WARNING = 1, // 预警区域
|
||
DANGER = 2 // 危险区域
|
||
};
|
||
|
||
// 碰撞风险信息
|
||
struct CollisionRisk {
|
||
std::string id1, id2; // 碰撞物体的ID
|
||
RiskLevel level; // 风险等级
|
||
double distance; // 当前距离
|
||
double minDistance; // 预测的最小距离
|
||
double relativeSpeed; // 相对速度
|
||
Vector2D relativeMotion; // 相对运动向量
|
||
WarningZoneType zoneType; // 预警区域类型
|
||
};
|
||
|
||
// 轨迹预测碰撞结果
|
||
struct CollisionPrediction {
|
||
bool willCollide; // 是否可能发生碰撞
|
||
double timeToCollision; // 到碰撞点的时间(秒)
|
||
Vector2D collisionPoint; // 可能的碰撞点
|
||
double minDistance; // 最小距离
|
||
};
|
||
|
||
class CollisionDetector {
|
||
public:
|
||
CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles);
|
||
|
||
// 更新交通数据
|
||
void updateTraffic(const std::vector<Aircraft>& aircraft,
|
||
const std::vector<Vehicle>& vehicles);
|
||
|
||
// 检测所有可能的碰撞
|
||
std::vector<CollisionRisk> detectCollisions();
|
||
|
||
// 基于轨迹预测的碰撞检测
|
||
CollisionPrediction predictTrajectoryCollision(
|
||
const Vector2D& pos1, double speed1, double heading1, // 物体1的位置、速度、航向
|
||
const Vector2D& pos2, double speed2, double heading2, // 物体2的位置、速度、航向
|
||
double size1, // 物体1的尺寸
|
||
double size2, // 物体2的尺寸
|
||
double timeWindow = 30.0 // 预测时间窗口(默认30秒)
|
||
) const;
|
||
|
||
private:
|
||
const AirportBounds& airportBounds_;
|
||
QuadTree<Vehicle> vehicleTree_;
|
||
std::vector<Aircraft> aircraftData_;
|
||
const ControllableVehicles* controllableVehicles_;
|
||
|
||
// 根据区域获取碰撞检测参数
|
||
AreaConfig getCollisionParams(const Vector2D& position) const {
|
||
auto areaType = airportBounds_.getAreaType(position);
|
||
return airportBounds_.getAreaConfig(areaType);
|
||
}
|
||
|
||
// 通用碰撞风险检测函数
|
||
bool checkCollisionRisk(
|
||
const Vector2D& pos1, double speed1, double heading1, const std::string& id1,
|
||
const Vector2D& pos2, double speed2, double heading2, const std::string& id2,
|
||
bool isAircraft1, bool isAircraft2) const;
|
||
|
||
// 检查航空器和车辆是否可能碰撞
|
||
bool checkAircraftVehicleCollision(const Aircraft& aircraft, const Vehicle& vehicle) const;
|
||
// 检查两辆车是否可能碰撞
|
||
bool checkVehicleCollision(const Vehicle& v1, const Vehicle& v2) const;
|
||
|
||
// 计算风险等级
|
||
RiskLevel calculateRiskLevel(double distance, const Vector2D& position,
|
||
bool isAircraft1, bool isAircraft2) const;
|
||
|
||
// 确定预警区域类型
|
||
WarningZoneType determineWarningZone(double distance, double threshold) const;
|
||
|
||
// 统一的风险评估函数
|
||
std::pair<RiskLevel, WarningZoneType> evaluateRisk(
|
||
double currentDistance,
|
||
const Vector2D& position,
|
||
bool isAircraft1,
|
||
bool isAircraft2,
|
||
bool willCollide) const;
|
||
|
||
// 计算相对运动
|
||
struct MovementVector {
|
||
double vx, vy;
|
||
|
||
MovementVector(double speed, double heading) {
|
||
// 标准方位角：正北为0度，顺时针增加
|
||
double radians = heading * M_PI / 180.0;
|
||
|
||
// 计算速度分量
|
||
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");
|
||
}
|
||
};
|
||
};
|
||
|
||
#endif // AIRPORT_DETECTOR_COLLISION_DETECTOR_H
|