ActiveProtect/Models/BasicGuidanceSystem.cs

using System;

namespace ActiveProtect.Models
{
    /// <summary>
    /// 制导系统接口
    /// </summary>
    public interface IGuidanceSystem
    {
        bool HasGuidance { get; }
        void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity);
        Vector3D GetGuidanceAcceleration();
    }

    /// <summary>
    /// 基础制导系统
    /// </summary>
    public class BasicGuidanceSystem : IGuidanceSystem
    {
        /// <summary>
        /// 是否有制导
        /// </summary>
        public bool HasGuidance { get; protected set; }

        /// <summary>
        /// 最大加速度
        /// </summary>
        public double MaxAcceleration { get; set; }

        /// <summary>
        /// 制导系数
        /// </summary>
        public double ProportionalNavigationCoefficient { get; set; }

        /// <summary>
        /// 导弹位置
        /// </summary>
        protected Vector3D Position { get; set; }

        /// <summary>
        /// 导弹速度
        /// </summary>
        protected Vector3D Velocity { get; set; }

        /// <summary>
        /// 制导加速度
        /// </summary>
        protected Vector3D GuidanceAcceleration { get; set; }

        public BasicGuidanceSystem(double maxAcceleration, double proportionalNavigationCoefficient)
        {
            HasGuidance = false;
            GuidanceAcceleration = Vector3D.Zero;
            Position = Vector3D.Zero;
            Velocity = Vector3D.Zero;
            MaxAcceleration = maxAcceleration;
            ProportionalNavigationCoefficient = proportionalNavigationCoefficient;
        }

        /// <summary>
        /// 更新制导系统
        /// </summary>
        /// <param name="deltaTime">时间步长</param>
        /// <param name="missilePosition">导弹位置</param>
        /// <param name="missileVelocity">导弹速度</param>
        public virtual void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
        {
            Position = missilePosition;
            Velocity = missileVelocity;            
        }

        /// <summary>
        /// 获取制导加速度
        /// </summary>
        /// <returns>制导加速度</returns>
        public Vector3D GetGuidanceAcceleration()
        {
            return GuidanceAcceleration;
        }

        /// <summary>
        /// 计算制导加速度
        /// </summary>
        /// <param name="deltaTime">时间步长</param>
        protected virtual void CalculateGuidanceAcceleration(double deltaTime)
        {
            // 基础制导系统不计算制导指令
            // 派生类应该重写这个方法来实现特定的制导逻辑
        }

        /// <summary>
        /// 计算比例导引加速度
        /// </summary>
        /// <param name="proportionalNavigationCoefficient">比例导引系数</param>
        /// <param name="missilePosition">导弹位置</param>
        /// <param name="missileVelocity">导弹速度</param>
        /// <param name="targetPosition">目标位置</param>
        /// <param name="targetVelocity">目标速度</param>
        /// <returns>比例导引加速度</returns>
        protected static Vector3D CalculateProportionalNavigation(double proportionalNavigationCoefficient, Vector3D missilePosition, Vector3D missileVelocity, Vector3D targetPosition, Vector3D targetVelocity)
        {
            // 预测时间（预测目标前进方向该时间后到达的位置，可以调整）
            double predictionTime = 0.01;

            // 预测目标位置
            Vector3D predictedTargetPosition = targetPosition + targetVelocity * predictionTime;

            Vector3D r = predictedTargetPosition - missilePosition;
            Vector3D v = targetVelocity - missileVelocity;
            
            Vector3D LOS = r.Normalize();
            Vector3D LOSRate = (v - (LOS * Vector3D.DotProduct(v, LOS))) / r.Magnitude();
            
            Vector3D acceleration = Vector3D.CrossProduct(Vector3D.CrossProduct(LOS, LOSRate), missileVelocity.Normalize()) * proportionalNavigationCoefficient * missileVelocity.Magnitude();
            
            return acceleration;
        }

        /// <summary>
        /// 四阶龙格库塔法计算导弹运动状态
        /// </summary>
        /// <param name="deltaTime">时间步长</param>
        /// <param name="position">导弹位置</param>
        /// <param name="velocity">导弹速度</param>
        /// <param name="acceleration">导弹加速度</param>
        /// <returns>新的位置和速度</returns>
        public static (Vector3D newPosition, Vector3D newVelocity) RungeKutta4(double deltaTime, Vector3D position, Vector3D velocity, Vector3D acceleration)
        {
            // 定义一个局部函数来计算加速度
            Vector3D AccelerationFunction(Vector3D pos, Vector3D vel)
            {
                // 这里可以添加更复杂的加速度计算，比如考虑空气阻力等
                return acceleration;
            }

            // 第一步
            Vector3D k1v = AccelerationFunction(position, velocity) * deltaTime;
            Vector3D k1r = velocity * deltaTime;

            // 第二步
            Vector3D k2v = AccelerationFunction(position + k1r * 0.5, velocity + k1v * 0.5) * deltaTime;
            Vector3D k2r = (velocity + k1v * 0.5) * deltaTime;

            // 第三步
            Vector3D k3v = AccelerationFunction(position + k2r * 0.5, velocity + k2v * 0.5) * deltaTime;
            Vector3D k3r = (velocity + k2v * 0.5) * deltaTime;

            // 第四步
            Vector3D k4v = AccelerationFunction(position + k3r, velocity + k3v) * deltaTime;
            Vector3D k4r = (velocity + k3v) * deltaTime;

            // 计算新的位置和速度
            Vector3D newPosition = position + (k1r + k2r * 2 + k3r * 2 + k4r) / 6;
            Vector3D newVelocity = velocity + (k1v + k2v * 2 + k3v * 2 + k4v) / 6;

            return (newPosition, newVelocity);
        }

        /// <summary>
        /// 获取制导系统状态
        /// </summary>
        /// <returns>制导系统状态</returns>
        public virtual string GetStatus()
        {
            return $"  导引头状态: 有制导={HasGuidance}, 位置={Position}, 速度={Velocity}, 制导加速度={GuidanceAcceleration}";
        }
    }
}