diff --git a/ThreatSource.Tests/src/Missile/TerminalSensitiveSubmunitionTests.cs b/ThreatSource.Tests/src/Missile/TerminalSensitiveSubmunitionTests.cs
index b613047..6ac43c5 100644
--- a/ThreatSource.Tests/src/Missile/TerminalSensitiveSubmunitionTests.cs
+++ b/ThreatSource.Tests/src/Missile/TerminalSensitiveSubmunitionTests.cs
@@ -4,6 +4,7 @@ using ThreatSource.Simulation;
using ThreatSource.Utils;
using ThreatSource.Simulation.Testing;
using ThreatSource.Target;
+using Xunit.Abstractions;
namespace ThreatSource.Tests.Missile
{
@@ -14,9 +15,11 @@ namespace ThreatSource.Tests.Missile
private readonly TerminalSensitiveSubmunition _submunition;
private readonly MissileProperties _properties;
private readonly Tank _tank;
+ private readonly ITestOutputHelper _output;
- public TerminalSensitiveSubmunitionTests()
+ public TerminalSensitiveSubmunitionTests(ITestOutputHelper output)
{
+ _output = output;
_simulationManager = new SimulationManager();
_testAdapter = new TestSimulationAdapter(_simulationManager);
_simulationManager.SetSimulationAdapter(_testAdapter);
@@ -168,5 +171,81 @@ namespace ThreatSource.Tests.Missile
Assert.Contains(part, status);
}
}
+
+ [Fact]
+ public void TestSensorAngleCorrection()
+ {
+ // Arrange
+ // 设置子弹位置为原点
+ _submunition.Position = new Vector3D(0, 0, 0);
+
+ // 设置目标位置为 (100, 0, 0)，即正东方向
+ _tank.Position = new Vector3D(100, 0, 0);
+
+ // 设置扫描角度为 90 度（正北为 0 度，顺时针），即向东
+ double spiralAngle = 90 * Math.PI / 180; // 转换为弧度
+
+ // 计算扫描方向（考虑锥角）
+ Vector3D scanDirection = new Vector3D(
+ Math.Cos(spiralAngle) * Math.Sin(30 * Math.PI / 180),
+ -Math.Cos(30 * Math.PI / 180),
+ Math.Sin(spiralAngle) * Math.Sin(30 * Math.PI / 180)
+ );
+
+ // 计算水平面上的扫描方向
+ Vector3D horizontalScanDirection = new Vector3D(
+ Math.Cos(spiralAngle),
+ 0,
+ Math.Sin(spiralAngle)
+ );
+
+ // 计算目标方向
+ Vector3D toTarget = (_tank.Position - _submunition.Position).Normalize();
+
+ // 计算水平面上的目标方向
+ Vector3D horizontalTargetDirection = new Vector3D(
+ toTarget.X,
+ 0,
+ toTarget.Z
+ ).Normalize();
+
+ // 计算水平面上的夹角
+ double horizontalAngle = Math.Acos(Vector3D.DotProduct(horizontalScanDirection, horizontalTargetDirection)) * 180 / Math.PI;
+
+ // 计算修正后的攻击角度
+ double attackAngle = spiralAngle - horizontalAngle * Math.PI / 180;
+
+ // 计算修正后的扫描方向（考虑锥角）
+ Vector3D correctedDirection = new Vector3D(
+ Math.Cos(attackAngle) * Math.Sin(30 * Math.PI / 180),
+ -Math.Cos(30 * Math.PI / 180),
+ Math.Sin(attackAngle) * Math.Sin(30 * Math.PI / 180)
+ ).Normalize();
+
+ // 计算水平面上的目标方向
+ Vector3D horizontalCorrectedDirection = new Vector3D(
+ correctedDirection.X,
+ 0,
+ correctedDirection.Z
+ ).Normalize();
+
+ // Assert
+ // 1. 修正前的扫描方向和目标之间应该有偏差
+ Assert.True(horizontalAngle > 0, $"扫描线和目标之间应该有偏差，实际夹角: {horizontalAngle}°");
+
+ // 2. 修正后的方向应该指向目标（水平面上的夹角接近0）
+ double correctedHorizontalAngle = Math.Acos(Vector3D.DotProduct(horizontalCorrectedDirection, horizontalTargetDirection)) * 180 / Math.PI;
+ Assert.True(correctedHorizontalAngle < 0.1, $"修正后的方向应该指向目标，实际夹角: {correctedHorizontalAngle}°");
+
+ // 输出调试信息
+ _output.WriteLine($"原始扫描角度: {spiralAngle * 180 / Math.PI:F2}°");
+ _output.WriteLine($"目标位置: {_tank.Position}");
+ _output.WriteLine($"水平扫描方向: {horizontalScanDirection}");
+ _output.WriteLine($"水平目标方向: {horizontalTargetDirection}");
+ _output.WriteLine($"水平面夹角: {horizontalAngle:F2}°");
+ _output.WriteLine($"修正后的攻击角度: {attackAngle * 180 / Math.PI:F2}°");
+ _output.WriteLine($"修正后的水平方向: {horizontalCorrectedDirection}");
+ _output.WriteLine($"修正后的水平夹角: {correctedHorizontalAngle:F2}°");
+ }
}
}
\ No newline at end of file
diff --git a/ThreatSource/src/MIssile/TerminalSensitiveSubmunition.cs b/ThreatSource/src/MIssile/TerminalSensitiveSubmunition.cs
index 47013d4..7e9eff3 100644
--- a/ThreatSource/src/MIssile/TerminalSensitiveSubmunition.cs
+++ b/ThreatSource/src/MIssile/TerminalSensitiveSubmunition.cs
@@ -141,13 +141,19 @@ namespace ThreatSource.Missile
///
private double AttackSpeed = 200;
+ ///
+ /// 是否已经探测到目标
+ ///
+ private bool isTargetDetected = false;
+
+ ///
+ /// 第一次探测到目标时记录的目标方位角
+ ///
+ private double? firstDetectionAngle = null;
+
///
/// 获取或设置最后一次检测到目标的时间
///
- ///
- /// 用于目标确认和跟踪
- /// null表示尚未检测到目标
- ///
private double? lastDetectionTime = null;
///
@@ -160,7 +166,7 @@ namespace ThreatSource.Missile
private double spiralAngle;
///
- /// 获取或设置扫描方向向量
+ /// 扫描方向向量
///
///
/// 表示传感器的当前指向
@@ -169,13 +175,22 @@ namespace ThreatSource.Missile
private Vector3D scanDirection;
///
- /// 获取或设置目标检测角度
+ /// 获取扫描方向向量
///
///
- /// 记录检测到目标时的角度
- /// 用于目标确认和重获
+ /// 表示传感器的当前指向
+ /// 用于目标探测和跟踪
///
- //private double detectionAngle;
+ public Vector3D ScanDirection => scanDirection;
+
+ ///
+ /// 目标ID
+ ///
+ ///
+ /// 记录目标的唯一标识符
+ /// 用于目标识别和跟踪
+ ///
+ public string TargetId { get; }
///
/// 红外探测器实例
@@ -213,15 +228,6 @@ namespace ThreatSource.Missile
///
private readonly LaserRangefinder rangefinder;
- ///
- /// 目标ID
- ///
- ///
- /// 记录目标的唯一标识符
- /// 用于目标识别和跟踪
- ///
- private readonly string TargetId;
-
///
/// 初始化末敏子弹的新实例
///
@@ -242,12 +248,12 @@ namespace ThreatSource.Missile
currentStage = SubmunitionStage.Separation;
spiralAngle = 0;
- scanDirection = new Vector3D(0, 0, 0);
- //detectionAngle = 0;
+ // 初始化扫描方向为正北
+ scanDirection = new Vector3D(0, 0, 1).Normalize();
// 初始化传感器
- infraredDetector = new InfraredDetector(this, 500, InfraredBand.Short, 1); // 红外探测器，探测距离 500 米，近红外，视场角0.5度
- radiometer = new MillimeterWaveRadiometer(this, 500, MillimeterWaveBand.Band3, 1); // 毫米波辐射计，探测距离500米，工作波段3mm，扫描视场角0.5度
+ infraredDetector = new InfraredDetector(this, 500, InfraredBand.Short, 2.5); // 红外探测器，探测距离 500 米，近红外，视场角1度
+ radiometer = new MillimeterWaveRadiometer(this, 500, MillimeterWaveBand.Band3, 2.5); // 毫米波辐射计，探测距离500米，工作波段3mm，扫描视场角1度
altimeter = new MillimeterWaveAltimeter(this, 1000, MillimeterWaveBand.Band8, 0.5, 25); // 毫米波测高仪，测量精度0.5米
rangefinder = new LaserRangefinder(this, 500, 1.06, 100, 0.5); // 激光测距仪，测量距离500米，波长1.06µm，测量频率100Hz，测量精度0.5米
}
@@ -430,28 +436,19 @@ namespace ThreatSource.Missile
return;
}
- // 更新螺旋角度
- double oldSpiralAngle = spiralAngle;
- spiralAngle = (spiralAngle + SpiralRotationSpeed * deltaTime) % (2 * Math.PI);
- Console.WriteLine($"[ANGLE] 螺旋角更新: {oldSpiralAngle * 180 / Math.PI:F2}°({oldSpiralAngle:F6}) -> {spiralAngle * 180 / Math.PI:F2}°({spiralAngle:F6})");
+ // 更新螺旋角度（顺时针旋转，标准方位角）
+ spiralAngle = (spiralAngle + SpiralRotationSpeed * deltaTime); // 顺时针角度增加
+ while (spiralAngle >= 2 * Math.PI)
+ {
+ spiralAngle -= 2 * Math.PI;
+ }
- // 计算水平速度分量
- double horizontalSpeed = VerticalDeclineSpeed * Math.Tan(ScanAngle);
- Vector3D horizontalVelocity = new(
- Math.Cos(spiralAngle) * horizontalSpeed,
- 0,
- Math.Sin(spiralAngle) * horizontalSpeed
- );
-
- // 更新速度，执行螺旋运动
- Velocity = new(horizontalVelocity.X, -VerticalDeclineSpeed, horizontalVelocity.Z);
-
- // 更新扫描方向（虽然这时候不开启探测器，但保持方向更新）
- scanDirection = new(
- Math.Cos(spiralAngle) * Math.Sin(ScanAngle),
+ // 更新扫描方向（标准方位角，顺时针为正）
+ scanDirection = new Vector3D(
+ Math.Sin(spiralAngle) * Math.Sin(ScanAngle),
-Math.Cos(ScanAngle),
- Math.Sin(spiralAngle) * Math.Sin(ScanAngle)
- );
+ Math.Cos(spiralAngle) * Math.Sin(ScanAngle)
+ ).Normalize();
// 如果距离小于等于目标探测距离，则进入目标探测阶段
if (((RangefinderSensorData)rangefinder.GetSensorData()).Distance <= TargetDetectionDistance)
@@ -491,56 +488,102 @@ namespace ThreatSource.Missile
return;
}
- // 更新螺旋角度
- double oldSpiralAngle = spiralAngle;
- spiralAngle = (spiralAngle + SpiralRotationSpeed * deltaTime) % (2 * Math.PI);
- if (lastDetectionTime != null)
+ // 更新螺旋角度（顺时针旋转）
+ spiralAngle = (spiralAngle + SpiralRotationSpeed * deltaTime); // 顺时针角度增加
+ while (spiralAngle >= 2 * Math.PI)
{
- Console.WriteLine($"[DEBUG] 螺旋角更新: {oldSpiralAngle * 180 / Math.PI:F2}° -> {spiralAngle * 180 / Math.PI:F2}°");
+ spiralAngle -= 2 * Math.PI;
}
- // 计算水平速度分量
- double horizontalSpeed = VerticalDeclineSpeed * Math.Tan(ScanAngle);
- Vector3D horizontalVelocity = new(
- Math.Cos(spiralAngle) * horizontalSpeed,
- 0,
- Math.Sin(spiralAngle) * horizontalSpeed
- );
-
- // 更新速度
- Velocity = new(horizontalVelocity.X, -VerticalDeclineSpeed, horizontalVelocity.Z);
-
// 计算扫描方向
- scanDirection = new(
- Math.Cos(spiralAngle) * Math.Sin(ScanAngle),
+ scanDirection = new Vector3D(
+ Math.Sin(spiralAngle) * Math.Sin(ScanAngle),
-Math.Cos(ScanAngle),
- Math.Sin(spiralAngle) * Math.Sin(ScanAngle)
- );
+ Math.Cos(spiralAngle) * Math.Sin(ScanAngle)
+ ).Normalize();
- bool isRadiationDetected = radiometer.GetSensorData() is RadiometerSensorData radiometerData && radiometerData.IsTargetDetected;
- bool isInfraredDetected = infraredDetector.GetSensorData() is InfraredSensorData infraredData && infraredData.IsTargetDetected;
-
- if (isRadiationDetected || isInfraredDetected)
+ // 获取传感器数据
+ RadiometerSensorData? radiometerData = null;
+ InfraredSensorData? infraredData = null;
+
+ if (radiometer.GetSensorData() is RadiometerSensorData rd)
{
- double currentTime = FlightTime;
- if (lastDetectionTime == null)
+ radiometerData = rd;
+ }
+ if (infraredDetector.GetSensorData() is InfraredSensorData id)
+ {
+ infraredData = id;
+ }
+
+ bool isRadiationDetected = radiometerData?.IsTargetDetected ?? false;
+ bool isInfraredDetected = infraredData?.IsTargetDetected ?? false;
+
+ // 如果还未探测到目标，进行首次探测
+ if (!isTargetDetected)
+ {
+ if (isRadiationDetected || isInfraredDetected)
{
- lastDetectionTime = currentTime;
- Console.WriteLine($"[DEBUG] 首次探测到目标:");
- }
- else
- {
- double timeSinceLastDetection = currentTime - lastDetectionTime.Value;
- if (timeSinceLastDetection >= 0.9 * 2 * Math.PI / SpiralRotationSpeed)
+ // 获取传感器的角度数据
+ double? radiometerAngle = radiometerData?.TargetAngle;
+ double? infraredAngle = infraredData?.TargetAngle;
+
+ // 使用有效的角度数据
+ if (radiometerAngle.HasValue || infraredAngle.HasValue)
{
- Console.WriteLine($"目标确认，进入攻击阶段。距离上次检测时间：{timeSinceLastDetection:F2}秒");
- currentStage = SubmunitionStage.Attack;
- return;
+ // 计算目标方位角
+ double targetAngle;
+ if (radiometerAngle.HasValue && infraredAngle.HasValue)
+ {
+ targetAngle = (radiometerAngle.Value + infraredAngle.Value) / 2;
+ }
+ else if (radiometerAngle.HasValue)
+ {
+ targetAngle = radiometerAngle.Value;
+ }
+ else
+ {
+ targetAngle = infraredAngle.Value;
+ }
+
+ // 记录首次探测信息
+ isTargetDetected = true;
+ lastDetectionTime = FlightTime;
+ firstDetectionAngle = targetAngle;
+ Console.WriteLine($"[DEBUG] 首次探测到目标，方位角: {firstDetectionAngle.Value * 180 / Math.PI:F2}°");
+ }
+ }
+ }
+ else // 已经探测到目标，等待转过一圈进行二次确认
+ {
+ double timeSinceLastDetection = FlightTime - lastDetectionTime.Value;
+ if (timeSinceLastDetection >= 0.75 * 2 * Math.PI / SpiralRotationSpeed) // 转过3/4圈以后
+ {
+ // 检查当前角度是否接近记录的目标方位角
+ double angleDiff = Math.Abs(spiralAngle - firstDetectionAngle.Value);
+ // 根据仿真步长和转速计算允许的角度误差
+ double allowedError = deltaTime * SpiralRotationSpeed / 2;
+ if (angleDiff <= allowedError)
+ {
+ if (isRadiationDetected || isInfraredDetected)
+ {
+ // 二次确认成功，进入攻击阶段
+ Console.WriteLine($"[DEBUG] 二次确认成功，进入攻击阶段，当前角度: {spiralAngle * 180 / Math.PI:F2}°");
+ currentStage = SubmunitionStage.Attack;
+ return;
+ }
+ else
+ {
+ // 二次确认失败，重新开始扫描
+ Console.WriteLine($"[DEBUG] 二次确认失败，重新开始扫描");
+ isTargetDetected = false;
+ lastDetectionTime = null;
+ firstDetectionAngle = null;
+ }
}
}
}
- // 使用模式匹配简化类型转换
+ // 检查自毁条件
if (rangefinder.GetSensorData() is RangefinderSensorData rangefinderData &&
rangefinderData.Distance <= SelfDestructHeight)
{
@@ -554,18 +597,28 @@ namespace ThreatSource.Missile
/// 时间步长，单位：秒
///
/// 攻击阶段处理：
- /// - 使用当前的螺旋角和锥角计算攻击方向
+ /// - 使用记录的攻击角度计算攻击方向
/// - 使用扫描方向作为攻击方向，弹丸没有传感器，只进行弹道运动
/// - 更新速度和位置
/// - 检查命中条件：使用纯几何计算
///
private void UpdateAttackStage(double deltaTime)
{
- // 使用当前的螺旋角和锥角计算攻击方向
+ // 关闭传感器
+ if (radiometer.IsActive)
+ {
+ radiometer.Deactivate();
+ }
+ if (infraredDetector.IsActive)
+ {
+ infraredDetector.Deactivate();
+ }
+
+ // 使用当前的spiralAngle计算攻击方向
scanDirection = new Vector3D(
- Math.Cos(spiralAngle) * Math.Sin(ScanAngle),
+ Math.Sin(spiralAngle) * Math.Sin(ScanAngle),
-Math.Cos(ScanAngle),
- Math.Sin(spiralAngle) * Math.Sin(ScanAngle)
+ Math.Cos(spiralAngle) * Math.Sin(ScanAngle)
).Normalize();
// 使用 AttackSpeed 设置攻击速度
@@ -621,14 +674,14 @@ namespace ThreatSource.Missile
/// 检测目标是否在视场内
///
/// 视场角，单位：度
- /// 如果目标在视场内返回true，否则返回false
+ /// 返回目标方位角。如果目标在视场内返回方位角，否则返回null
///
/// 检测过程：
/// - 获取目标位置
/// - 计算目标方向
/// - 判断是否在视场内
///
- public bool DetectTarget(double fieldOfView)
+ public double? DetectTarget(double fieldOfView)
{
// 获取目标对象
SimulationElement target = SimulationManager.GetEntityById(TargetId) as SimulationElement ?? throw new Exception("目标不存在");
@@ -651,18 +704,28 @@ namespace ThreatSource.Missile
vertices[2] = targetPosition - (targetForward * (targetLength/2)) + (targetRight * (targetWidth/2)); // 右后
vertices[3] = targetPosition - (targetForward * (targetLength/2)) - (targetRight * (targetWidth/2)); // 左后
- // 检查是否至少有一个顶点在视场内
+ // 计算目标方位角（弧度值）
+ Vector3D toTarget = (targetPosition - Position).Normalize();
+ double targetAzimuth = Math.Atan2(toTarget.X, toTarget.Z); // 修改为 atan2(X, Z) 使正北为0度
+ if (targetAzimuth < 0)
+ {
+ targetAzimuth += 2 * Math.PI; // 转换到 [0, 2π]
+ }
+
+ // 视场检测
double cosFieldOfView = Math.Cos(fieldOfView * Math.PI / 180);
foreach(var vertex in vertices)
{
Vector3D toVertex = (vertex - Position).Normalize();
if(Vector3D.DotProduct(scanDirection, toVertex) > cosFieldOfView)
{
- return true;
+ // 如果目标的任何一个顶点在视场角内，返回目标方位角
+ return targetAzimuth;
}
}
- return false;
+ // 如果目标不在视场角内，返回null
+ return null;
}
///
@@ -733,11 +796,8 @@ namespace ThreatSource.Missile
$"运行状态: {currentStage}\n" +
$"扫描角度: {spiralAngle * 180 / Math.PI:F2}°, 弧度值: {spiralAngle:F6}\n" +
$"目标检测: {lastDetectionTime != null}";
-
- if (IsSensorsJammed())
- {
- status += "\n传感器状态: 受到干扰";
- }
+
+ //var status = $"";
return status;
}
diff --git a/ThreatSource/src/Sensor/InfraredDetector.cs b/ThreatSource/src/Sensor/InfraredDetector.cs
index dbe83af..27fe39d 100644
--- a/ThreatSource/src/Sensor/InfraredDetector.cs
+++ b/ThreatSource/src/Sensor/InfraredDetector.cs
@@ -1,6 +1,7 @@
-using ThreatSource.Missile;
using ThreatSource.Utils;
using ThreatSource.Jamming;
+using ThreatSource.Simulation;
+using ThreatSource.Missile;
namespace ThreatSource.Sensor
{
@@ -136,7 +137,14 @@ namespace ThreatSource.Sensor
private double GetCurrentDirectionRadiationIntensity()
{
// 获取当前方向上的辐射强度，如果检测到目标，则返回目标的辐射强度，否则返回0
- return submunition.DetectTarget(FieldOfView) ? 100 : 0;
+ var angle = submunition.DetectTarget(FieldOfView);
+ if (angle != null)
+ {
+ sensorData.TargetAngle = angle;
+ return 100;
+ }
+ sensorData.TargetAngle = null;
+ return 0;
}
///
diff --git a/ThreatSource/src/Sensor/LaserRangefinder.cs b/ThreatSource/src/Sensor/LaserRangefinder.cs
index 18b14aa..7c4df6e 100644
--- a/ThreatSource/src/Sensor/LaserRangefinder.cs
+++ b/ThreatSource/src/Sensor/LaserRangefinder.cs
@@ -127,7 +127,7 @@ namespace ThreatSource.Sensor
currentDistance = slantRange + (2 * new Random().NextDouble() - 1) * Accuracy;
// 调试输出
- Console.WriteLine($"激光测距仪 - 垂直距离: {verticalDistance:F2}m, 水平距离: {horizontalDistance:F2}m, 斜距: {slantRange:F2}m, 测量距离: {currentDistance:F2}m");
+ //Console.WriteLine($"激光测距仪 - 垂直距离: {verticalDistance:F2}m, 水平距离: {horizontalDistance:F2}m, 斜距: {slantRange:F2}m, 测量距离: {currentDistance:F2}m");
}
}
diff --git a/ThreatSource/src/Sensor/MillimeterWaveRadiometer.cs b/ThreatSource/src/Sensor/MillimeterWaveRadiometer.cs
index 444555a..83e6c07 100644
--- a/ThreatSource/src/Sensor/MillimeterWaveRadiometer.cs
+++ b/ThreatSource/src/Sensor/MillimeterWaveRadiometer.cs
@@ -1,6 +1,7 @@
-using ThreatSource.Missile;
using ThreatSource.Utils;
using ThreatSource.Jamming;
+using ThreatSource.Simulation;
+using ThreatSource.Missile;
namespace ThreatSource.Sensor
{
@@ -163,7 +164,14 @@ namespace ThreatSource.Sensor
private double GetCurrentDirectionRadiationTemperature()
{
// 获取当前方向上的辐射温度，如果检测到目标，则返回天空背景辐射的反射温度，否则返回300K
- return submunition.DetectTarget(FieldOfView) ? 90 : 300;
+ var angle = submunition.DetectTarget(FieldOfView);
+ if (angle != null)
+ {
+ sensorData.TargetAngle = angle;
+ return 90;
+ }
+ sensorData.TargetAngle = null;
+ return 300;
}
///
diff --git a/ThreatSource/src/Sensor/SensorData.cs b/ThreatSource/src/Sensor/SensorData.cs
index 8d9d4eb..212fdca 100644
--- a/ThreatSource/src/Sensor/SensorData.cs
+++ b/ThreatSource/src/Sensor/SensorData.cs
@@ -51,6 +51,16 @@ namespace ThreatSource.Sensor
/// 用于目标存在性判断
///
public bool IsTargetDetected { get; set; }
+
+ ///
+ /// 获取或设置目标的方位角，单位：弧度
+ ///
+ ///
+ /// 记录目标的方位角
+ /// 用于目标定位和跟踪
+ /// 当未检测到目标时为null
+ ///
+ public double? TargetAngle { get; set; }
}
///
@@ -82,6 +92,16 @@ namespace ThreatSource.Sensor
/// 用于目标存在性判断
///
public bool IsTargetDetected { get; set; }
+
+ ///
+ /// 获取或设置目标的方位角，单位：弧度
+ ///
+ ///
+ /// 记录目标的方位角
+ /// 用于目标定位和跟踪
+ /// 当未检测到目标时为null
+ ///
+ public double? TargetAngle { get; set; }
}
///