tian/ThreatSourceLibaray
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

激光测距仪

Browse Source
This commit is contained in:
Tian jianyong 2025-01-09 19:32:25 +08:00
parent 3eb8194620
commit 955be5cf97
6 changed files with 274 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
ThreatSource.Tests/src/Missile/TerminalSensitiveSubmunitionTests.cs
View File

@ -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}°");
}
}
}

246
ThreatSource/src/MIssile/TerminalSensitiveSubmunition.cs
View File

@ -141,13 +141,19 @@ namespace ThreatSource.Missile
/// </remarks>
private double AttackSpeed = 200;
/// <summary>
/// 是否已经探测到目标
/// </summary>
private bool isTargetDetected = false;
/// <summary>
/// 第一次探测到目标时记录的目标方位角
/// </summary>
private double? firstDetectionAngle = null;
/// <summary>
/// 获取或设置最后一次检测到目标的时间
/// </summary>
/// <remarks>
/// 用于目标确认和跟踪
/// null表示尚未检测到目标
/// </remarks>
private double? lastDetectionTime = null;
/// <summary>
@ -160,7 +166,7 @@ namespace ThreatSource.Missile
private double spiralAngle;
/// <summary>
/// 获取或设置扫描方向向量
/// 扫描方向向量
/// </summary>
/// <remarks>
/// 表示传感器的当前指向
@ -169,13 +175,22 @@ namespace ThreatSource.Missile
private Vector3D scanDirection;
/// <summary>
/// 获取或设置目标检测角度
/// 获取扫描方向向量
/// </summary>
/// <remarks>
/// 记录检测到目标时的角度
/// 用于目标确认和重获
/// 表示传感器的当前指向
/// 用于目标探测和跟踪
/// </remarks>
//private double detectionAngle;
public Vector3D ScanDirection => scanDirection;
/// <summary>
/// 目标ID
/// </summary>
/// <remarks>
/// 记录目标的唯一标识符
/// 用于目标识别和跟踪
/// </remarks>
public string TargetId { get; }
/// <summary>
/// 红外探测器实例
@ -213,15 +228,6 @@ namespace ThreatSource.Missile
/// </remarks>
private readonly LaserRangefinder rangefinder;
/// <summary>
/// 目标ID
/// </summary>
/// <remarks>
/// 记录目标的唯一标识符
/// 用于目标识别和跟踪
/// </remarks>
private readonly string TargetId;
/// <summary>
/// 初始化末敏子弹的新实例
/// </summary>
@ -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
/// <param name="deltaTime">时间步长，单位：秒</param>
/// <remarks>
/// 攻击阶段处理：
/// - 使用当前的螺旋角和锥角计算攻击方向
/// - 使用记录的攻击角度计算攻击方向
/// - 使用扫描方向作为攻击方向，弹丸没有传感器，只进行弹道运动
/// - 更新速度和位置
/// - 检查命中条件：使用纯几何计算
/// </remarks>
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
/// 检测目标是否在视场内
/// </summary>
/// <param name="fieldOfView">视场角，单位：度</param>
/// <returns>如果目标在视场内返回true，否则返回false</returns>
/// <returns>返回目标方位角。如果目标在视场内返回方位角，否则返回null</returns>
/// <remarks>
/// 检测过程：
/// - 获取目标位置
/// - 计算目标方向
/// - 判断是否在视场内
/// </remarks>
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;
}
/// <summary>
@ -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;
}

12
ThreatSource/src/Sensor/InfraredDetector.cs
View File

@ -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;
}
/// <summary>

2
ThreatSource/src/Sensor/LaserRangefinder.cs
View File

@ -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");
}
}

12
ThreatSource/src/Sensor/MillimeterWaveRadiometer.cs
View File

@ -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;
}
/// <summary>

20
ThreatSource/src/Sensor/SensorData.cs
View File

@ -51,6 +51,16 @@ namespace ThreatSource.Sensor
/// 用于目标存在性判断
/// </remarks>
public bool IsTargetDetected { get; set; }
/// <summary>
/// 获取或设置目标的方位角，单位：弧度
/// </summary>
/// <remarks>
/// 记录目标的方位角
/// 用于目标定位和跟踪
/// 当未检测到目标时为null
/// </remarks>
public double? TargetAngle { get; set; }
}
/// <summary>
@ -82,6 +92,16 @@ namespace ThreatSource.Sensor
/// 用于目标存在性判断
/// </remarks>
public bool IsTargetDetected { get; set; }
/// <summary>
/// 获取或设置目标的方位角，单位：弧度
/// </summary>
/// <remarks>
/// 记录目标的方位角
/// 用于目标定位和跟踪
/// 当未检测到目标时为null
/// </remarks>
public double? TargetAngle { get; set; }
}
/// <summary>