From 955be5cf97774a26f87745f107bceaf00af58840 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Thu, 9 Jan 2025 19:32:25 +0800 Subject: [PATCH] =?UTF-8?q?=E6=BF=80=E5=85=89=E6=B5=8B=E8=B7=9D=E4=BB=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../TerminalSensitiveSubmunitionTests.cs | 81 +++++- .../MIssile/TerminalSensitiveSubmunition.cs | 246 +++++++++++------- ThreatSource/src/Sensor/InfraredDetector.cs | 12 +- ThreatSource/src/Sensor/LaserRangefinder.cs | 2 +- .../src/Sensor/MillimeterWaveRadiometer.cs | 12 +- ThreatSource/src/Sensor/SensorData.cs | 20 ++ 6 files changed, 274 insertions(+), 99 deletions(-) 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; } } ///