143 lines
5.1 KiB
C++
143 lines
5.1 KiB
C++
#include "SafetyZone.h"
|
|
#include "utils/Logger.h"
|
|
#include "config/SystemConfig.h"
|
|
#include <cmath>
|
|
|
|
namespace collision {
|
|
|
|
SafetyZone::SafetyZone(const Vector2D& center, double aircraftRadius, double specialVehicleRadius)
|
|
: center_(center)
|
|
, aircraftRadius_(aircraftRadius)
|
|
, specialVehicleRadius_(specialVehicleRadius)
|
|
, currentRadius_(0.0)
|
|
, state_(SafetyZoneState::INACTIVE)
|
|
, type_(SafetyZoneType::NONE) {
|
|
|
|
Logger::debug("创建安全区: center=(", center_.x, ",", center_.y,
|
|
"), aircraftRadius=", aircraftRadius_,
|
|
", specialVehicleRadius=", specialVehicleRadius_);
|
|
}
|
|
|
|
bool SafetyZone::isInZone(const Vector2D& point) const {
|
|
// 如果未设置类型，直接返回 false
|
|
if (type_ == SafetyZoneType::NONE) {
|
|
return false;
|
|
}
|
|
|
|
// 计算点到中心的距离
|
|
double dx = point.x - center_.x;
|
|
double dy = point.y - center_.y;
|
|
double distance = std::sqrt(dx * dx + dy * dy);
|
|
|
|
// 使用当前设置的安全区半径
|
|
return distance <= currentRadius_;
|
|
}
|
|
|
|
void SafetyZone::resetType() {
|
|
type_ = SafetyZoneType::NONE;
|
|
currentRadius_ = 0.0;
|
|
Logger::debug("重置安全区类型为 NONE");
|
|
}
|
|
|
|
bool SafetyZone::trySetType(const MovingObject& object) {
|
|
// 判断目标类型
|
|
bool isAircraft = object.isAircraft();
|
|
bool isSpecialVehicle = object.isSpecialVehicle();
|
|
|
|
// 如果既不是飞机也不是特勤车，返回 false
|
|
if (!isAircraft && !isSpecialVehicle) {
|
|
return false;
|
|
}
|
|
|
|
// 如果安全区已有类型，检查是否可以改变
|
|
if (type_ != SafetyZoneType::NONE) {
|
|
// 如果当前类型与目标类型不匹配，不允许改变
|
|
if ((type_ == SafetyZoneType::AIRCRAFT && !isAircraft) ||
|
|
(type_ == SafetyZoneType::VEHICLE && !isSpecialVehicle)) {
|
|
return false;
|
|
}
|
|
// 如果类型匹配，保持当前类型
|
|
return true;
|
|
}
|
|
|
|
// 设置新的类型和尺寸
|
|
if (isAircraft) {
|
|
type_ = SafetyZoneType::AIRCRAFT;
|
|
currentRadius_ = aircraftRadius_;
|
|
state_ = SafetyZoneState::ACTIVE; // 激活安全区
|
|
Logger::debug("安全区设置为飞机类型，半径: ", currentRadius_);
|
|
} else {
|
|
type_ = SafetyZoneType::VEHICLE;
|
|
currentRadius_ = specialVehicleRadius_;
|
|
state_ = SafetyZoneState::ACTIVE; // 激活安全区
|
|
Logger::debug("安全区设置为特勤车类型，半径: ", currentRadius_);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool SafetyZone::isObjectInZone(const MovingObject& object) const {
|
|
// 如果是无人车，直接返回 false
|
|
if (object.isUnmannedVehicle()) {
|
|
Logger::debug("目标 ", object.id, " 是无人车，不触发安全区");
|
|
return false;
|
|
}
|
|
|
|
// 从 SystemConfig 获取物体尺寸（半长度）
|
|
const auto& config = SystemConfig::instance().collision_detection.prediction;
|
|
double objectRadius = 0.0;
|
|
|
|
// 获取物体半径
|
|
if (object.isAircraft()) {
|
|
objectRadius = config.aircraft_size / 2.0;
|
|
Logger::debug("目标 ", object.id, " 是飞机，尺寸半径: ", objectRadius);
|
|
} else if (object.isSpecialVehicle()) {
|
|
objectRadius = config.vehicle_size / 2.0;
|
|
Logger::debug("目标 ", object.id, " 是特勤车，尺寸半径: ", objectRadius);
|
|
} else {
|
|
return false;
|
|
}
|
|
|
|
// 如果安全区类型未设置，尝试设置类型
|
|
if (type_ == SafetyZoneType::NONE) {
|
|
// 临时设置类型和半径来检查是否在区内
|
|
SafetyZoneType tempType = object.isAircraft() ? SafetyZoneType::AIRCRAFT : SafetyZoneType::VEHICLE;
|
|
double tempRadius = object.isAircraft() ? aircraftRadius_ : specialVehicleRadius_;
|
|
|
|
// 暂存当前值
|
|
auto currentType = type_;
|
|
auto currentRadius = currentRadius_;
|
|
|
|
// 临时设置类型和半径
|
|
const_cast<SafetyZone*>(this)->type_ = tempType;
|
|
const_cast<SafetyZone*>(this)->currentRadius_ = tempRadius;
|
|
|
|
// 检查是否在区内
|
|
bool inZone = isInZone(object.position);
|
|
|
|
// 如果在区内，保持新的类型和半径，否则恢复原值
|
|
if (!inZone) {
|
|
const_cast<SafetyZone*>(this)->type_ = currentType;
|
|
const_cast<SafetyZone*>(this)->currentRadius_ = currentRadius;
|
|
} else {
|
|
// 设置状态为激活
|
|
const_cast<SafetyZone*>(this)->state_ = SafetyZoneState::ACTIVE;
|
|
Logger::debug("安全区设置为", object.isAircraft() ? "飞机" : "特勤车", "类型，半径: ", tempRadius);
|
|
}
|
|
|
|
return inZone;
|
|
}
|
|
|
|
// 如果类型已设置，检查是否匹配
|
|
if ((type_ == SafetyZoneType::AIRCRAFT && !object.isAircraft()) ||
|
|
(type_ == SafetyZoneType::VEHICLE && !object.isSpecialVehicle())) {
|
|
Logger::debug("目标 ", object.id, " 类型与安全区类型不匹配，当前安全区类型: ",
|
|
type_ == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车");
|
|
return false;
|
|
}
|
|
|
|
// 使用 isInZone 检查是否在区内
|
|
return isInZone(object.position);
|
|
}
|
|
|
|
} // namespace collision
|