348 lines
13 KiB
Plaintext
348 lines
13 KiB
Plaintext
using System;
|
||
using System.Collections.Generic;
|
||
using ActiveProtect.SimulationEnvironment;
|
||
|
||
namespace ActiveProtect.Models
|
||
{
|
||
public class TerminalSensitiveMissile : MissileBase
|
||
{
|
||
// 定义末敏弹工作阶段的枚举
|
||
private enum TerminalSensitiveStage
|
||
{
|
||
Launch, // 发射阶段
|
||
Separation, // 分离阶段
|
||
Deceleration, // 减速阶段
|
||
SpiralScan, // 螺旋扫描阶段
|
||
Attack, // 攻击阶段
|
||
SelfDestruct // 自毁阶段
|
||
}
|
||
|
||
// 当前工作阶段
|
||
private TerminalSensitiveStage currentWorkingStage;
|
||
|
||
// 阶段策略字典，用于存储不同阶段的更新策略
|
||
private Dictionary<TerminalSensitiveStage, Action<double>> stageStrategies;
|
||
|
||
// 螺旋扫描相关参数
|
||
private double spiralAngle;
|
||
private const double SpiralRotationSpeed = 4 * Math.PI; // 2 rotations per second
|
||
private const double VerticalDescentSpeed = 10; // 10 m/s
|
||
private const double ScanAngle = 30 * Math.PI / 180; // 30 degrees in radians
|
||
|
||
// 各阶段的距离和高度参数
|
||
private const double SeparationDistance = 1000; // 分离距离 1000米
|
||
private const double SeparationHeight = 1000; // 分离高度 1000米
|
||
private const double DecelerationHeight = 400; // 减速高度 400米
|
||
private const double ScanningHeight = 200; // 扫描高度 200米
|
||
private const double SelfDestructHeight = 20; // 自毁高度 20米
|
||
|
||
// 扫描和目标检测相关参数
|
||
private Vector3D lastScanDirection;
|
||
private bool targetDetected;
|
||
private double detectionAngle;
|
||
|
||
// 目标位置和分离位置
|
||
private Vector3D TargetPosition;
|
||
private Vector3D SeparationPosition;
|
||
|
||
// 制导指令
|
||
public Vector3D GuidanceCommand { get; private set; }
|
||
|
||
// 传感器
|
||
private InfraredDetector infraredDetector;
|
||
private MillimeterWaveRadiometer radiometer;
|
||
private MillimeterWaveAltimeter altimeter;
|
||
private LaserRangefinder rangefinder;
|
||
|
||
// 构造函数
|
||
public TerminalSensitiveMissile(string id, MissileConfig missileConfig, string targetId, ISimulationManager simulationManager)
|
||
: base(id, missileConfig, simulationManager)
|
||
{
|
||
// 初始化参数
|
||
spiralAngle = 0;
|
||
lastScanDirection = Vector3D.Zero;
|
||
targetDetected = false;
|
||
detectionAngle = 0;
|
||
GuidanceCommand = Vector3D.Zero;
|
||
|
||
// 获取目标位置
|
||
TargetPosition = SimulationManager.GetEntityById(targetId).Position;
|
||
|
||
// 计算分离位置
|
||
SeparationPosition = CalculatePointOnTrajectory(Position, TargetPosition, SeparationDistance, SeparationHeight);
|
||
|
||
// 初始化阶段策略字典
|
||
stageStrategies = new Dictionary<TerminalSensitiveStage, Action<double>>
|
||
{
|
||
{ TerminalSensitiveStage.Launch, UpdateLaunchStage },
|
||
{ TerminalSensitiveStage.Separation, UpdateSeparationStage },
|
||
{ TerminalSensitiveStage.Deceleration, UpdateDecelerationStage },
|
||
{ TerminalSensitiveStage.SpiralScan, UpdateSpiralScanStage },
|
||
{ TerminalSensitiveStage.Attack, UpdateAttackStage },
|
||
{ TerminalSensitiveStage.SelfDestruct, UpdateSelfDestructStage }
|
||
};
|
||
|
||
// 设置初始阶段
|
||
currentWorkingStage = TerminalSensitiveStage.Launch;
|
||
|
||
// 初始化传感器
|
||
infraredDetector = new InfraredDetector(Position, Orientation, 1000, 30);
|
||
radiometer = new MillimeterWaveRadiometer(Position, Orientation, 94e9, 1e-6);
|
||
altimeter = new MillimeterWaveAltimeter(Position, Orientation, 1000, 0.1);
|
||
rangefinder = new LaserRangefinder(Position, Orientation, 5000, 1000);
|
||
}
|
||
|
||
// 更新方法，每帧调用
|
||
public override void Update(double deltaTime)
|
||
{
|
||
if (!IsActive) return;
|
||
|
||
if (ShouldSelfDestruct())
|
||
{
|
||
SelfDestruct();
|
||
return;
|
||
}
|
||
|
||
Console.WriteLine($"末敏弹 {Id} 当前位置: {Position}");
|
||
Console.WriteLine($"末敏弹 {Id} 分离位置: {SeparationPosition}");
|
||
|
||
// 更新传感器
|
||
infraredDetector.Update(deltaTime);
|
||
radiometer.Update(deltaTime);
|
||
altimeter.Update(deltaTime);
|
||
rangefinder.Update(deltaTime);
|
||
|
||
// 使用当前阶段的策略更新导弹状态
|
||
if (stageStrategies.TryGetValue(currentWorkingStage, out var updateStrategy))
|
||
{
|
||
updateStrategy(deltaTime);
|
||
}
|
||
}
|
||
|
||
// 发射阶段更新方法
|
||
private void UpdateLaunchStage(double deltaTime)
|
||
{
|
||
// 无动力抛物线飞行
|
||
ApplyGravity(deltaTime);
|
||
|
||
base.UpdateMotionState(deltaTime, GuidanceCommand);
|
||
|
||
if (Vector3D.Distance(Position, SeparationPosition) <= 200)
|
||
{
|
||
currentWorkingStage = TerminalSensitiveStage.Separation;
|
||
}
|
||
}
|
||
|
||
// 分离阶段更新方法
|
||
private void UpdateSeparationStage(double deltaTime)
|
||
{
|
||
Console.WriteLine($"末敏弹 {Id} 进入分离");
|
||
|
||
// 根据速度大小调整制导指令
|
||
switch(Velocity.Magnitude())
|
||
{
|
||
case >= 200:
|
||
GuidanceCommand = new Vector3D(400, -400, 0);
|
||
break;
|
||
case >= 100:
|
||
GuidanceCommand = new Vector3D(30, -30, 0);
|
||
break;
|
||
case >= 50:
|
||
GuidanceCommand = new Vector3D(20, -20, 0);
|
||
break;
|
||
case >= 20:
|
||
GuidanceCommand = new Vector3D(5, -10, 0);
|
||
break;
|
||
default:
|
||
GuidanceCommand = new Vector3D(0, -10, 0);
|
||
break;
|
||
}
|
||
base.UpdateMotionState(deltaTime, GuidanceCommand);
|
||
|
||
if (Position.Y <= DecelerationHeight)
|
||
{
|
||
currentWorkingStage = TerminalSensitiveStage.Deceleration;
|
||
}
|
||
}
|
||
|
||
// 减速阶段更新方法
|
||
private void UpdateDecelerationStage(double deltaTime)
|
||
{
|
||
Console.WriteLine($"末敏弹 {Id} 进入减速");
|
||
// 减速下降
|
||
Velocity = new Vector3D(0, -VerticalDescentSpeed, 0);
|
||
Position += Velocity * deltaTime;
|
||
|
||
if (Position.Y <= ScanningHeight)
|
||
{
|
||
currentWorkingStage = TerminalSensitiveStage.SpiralScan;
|
||
}
|
||
}
|
||
|
||
// 螺旋扫描阶段更新方法
|
||
private void UpdateSpiralScanStage(double deltaTime)
|
||
{
|
||
Console.WriteLine($"末敏弹 {Id} 进入螺旋扫描");
|
||
|
||
spiralAngle += SpiralRotationSpeed * deltaTime;
|
||
|
||
// 计算水平速度分量
|
||
double horizontalSpeed = VerticalDescentSpeed * Math.Tan(ScanAngle);
|
||
Vector3D horizontalVelocity = new Vector3D(
|
||
Math.Cos(spiralAngle) * horizontalSpeed,
|
||
0,
|
||
Math.Sin(spiralAngle) * horizontalSpeed
|
||
);
|
||
|
||
// 更新速度
|
||
Velocity = new Vector3D(horizontalVelocity.X, -VerticalDescentSpeed, horizontalVelocity.Z);
|
||
|
||
// 计算扫描方向
|
||
Vector3D scanDirection = new Vector3D(
|
||
Math.Cos(spiralAngle) * Math.Sin(ScanAngle),
|
||
-Math.Cos(ScanAngle),
|
||
Math.Sin(spiralAngle) * Math.Sin(ScanAngle)
|
||
);
|
||
|
||
if (DetectTarget(scanDirection))
|
||
{
|
||
if (!targetDetected)
|
||
{
|
||
targetDetected = true;
|
||
detectionAngle = spiralAngle;
|
||
}
|
||
else if (Math.Abs(spiralAngle - detectionAngle) < 0.1) // 允许一定的误差
|
||
{
|
||
currentWorkingStage = TerminalSensitiveStage.Attack;
|
||
}
|
||
}
|
||
|
||
lastScanDirection = scanDirection;
|
||
|
||
if (Position.Y <= SelfDestructHeight)
|
||
{
|
||
currentWorkingStage = TerminalSensitiveStage.SelfDestruct;
|
||
}
|
||
}
|
||
|
||
// 攻击阶段更新方法
|
||
private void UpdateAttackStage(double deltaTime)
|
||
{
|
||
// 攻击逻辑
|
||
Vector3D targetPosition = SimulationManager.GetEntityById(TargetId).Position;
|
||
Vector3D direction = (targetPosition - Position).Normalize();
|
||
Velocity = direction * MaxSpeed;
|
||
|
||
if (Vector3D.Distance(Position, targetPosition) < 1)
|
||
{
|
||
Explode();
|
||
}
|
||
}
|
||
|
||
// 自毁阶段更新方法
|
||
private void UpdateSelfDestructStage(double deltaTime)
|
||
{
|
||
// 实现自毁逻辑
|
||
Explode();
|
||
}
|
||
|
||
// 目标检测方法
|
||
private bool DetectTarget(Vector3D scanDirection)
|
||
{
|
||
Vector3D targetPosition = SimulationManager.GetEntityById(TargetId).Position;
|
||
Vector3D toTarget = (targetPosition - Position).Normalize();
|
||
|
||
// 检查目标是否在扫描线上
|
||
return Vector3D.DotProduct(scanDirection, toTarget) > Math.Cos(0.5 * Math.PI / 180); // 允许0.5度的误差
|
||
}
|
||
|
||
// 应用重力
|
||
private void ApplyGravity(double deltaTime)
|
||
{
|
||
GuidanceCommand = new Vector3D(0, -9.81, 0);
|
||
}
|
||
|
||
// 获取导弹状态
|
||
public override string GetStatus()
|
||
{
|
||
string baseStatus = base.GetStatus().Replace("导弹", "末敏弹");
|
||
string additionalStatus = $"\n 扫描阶段: {(currentWorkingStage == TerminalSensitiveStage.SpiralScan ? "有效" : "无效")} \n";
|
||
return baseStatus + additionalStatus;
|
||
}
|
||
|
||
// 激活导弹
|
||
public override void Activate()
|
||
{
|
||
base.Activate();
|
||
// 可以在这里添加订阅事件的代码
|
||
}
|
||
|
||
// 停用导弹
|
||
public override void Deactivate()
|
||
{
|
||
base.Deactivate();
|
||
// 可以在这里添加取消订阅事件的代码
|
||
}
|
||
|
||
// 静态方法：计算发射参数
|
||
public static (double angle, double distance) CalculateLaunchParameters(double initialVelocity, double targetHeight, double targetDistance)
|
||
{
|
||
double g = 9.8; // 重力加速度
|
||
double v = initialVelocity;
|
||
double y = targetHeight;
|
||
double x = targetDistance;
|
||
|
||
// 计算发射角度（弧度）
|
||
double angle = Math.Atan((v * v + Math.Sqrt(Math.Pow(v, 4) - g * (g * x * x + 2 * y * v * v))) / (g * x));
|
||
|
||
// 计算水平飞行距离
|
||
double t = x / (v * Math.Cos(angle));
|
||
double totalDistance = v * Math.Cos(angle) * t;
|
||
|
||
return (angle, totalDistance);
|
||
}
|
||
|
||
// 静态方法：计算发射位置
|
||
public static Vector3D CalculateLaunchPosition(Vector3D knownPoint, Vector3D velocity)
|
||
{
|
||
double g = 9.8; // 重力加速度
|
||
double x = knownPoint.X;
|
||
double y = knownPoint.Y;
|
||
double vx = velocity.X;
|
||
double vy = velocity.Y;
|
||
|
||
// 计算时间 t
|
||
double t = (-vy - Math.Sqrt(vy * vy + 2 * g * y)) / -g;
|
||
|
||
// 计算发射点的 x 坐标
|
||
double launchX = x - vx * t;
|
||
|
||
// 发射点的 y 和 z 坐标为 0
|
||
return new Vector3D(launchX, 0, 0);
|
||
}
|
||
|
||
// 静态方法：计算轨迹上的点
|
||
public static Vector3D CalculatePointOnTrajectory(Vector3D launchPosition, Vector3D targetPosition, double distanceFromTarget, double height)
|
||
{
|
||
// 确保发射位置和目标位置的y坐标为0
|
||
launchPosition = new Vector3D(launchPosition.X, 0, launchPosition.Z);
|
||
targetPosition = new Vector3D(targetPosition.X, 0, targetPosition.Z);
|
||
|
||
// 计算发射方向向量
|
||
Vector3D direction = targetPosition - launchPosition;
|
||
|
||
// 计算总距离
|
||
double totalDistance = direction.Magnitude();
|
||
|
||
// 归一化方向向量
|
||
direction = direction.Normalize();
|
||
|
||
// 计算所需点的位置
|
||
Vector3D pointOnTrajectory = targetPosition - direction * distanceFromTarget;
|
||
|
||
// 返回结果，y坐标设置为指定的高度
|
||
return new Vector3D(pointOnTrajectory.X, height, pointOnTrajectory.Z);
|
||
}
|
||
}
|
||
}
|