ActiveProtect/Models/BasicGuidanceSystem.cs

using System;

namespace ActiveProtect.Models
{
    public class BasicGuidanceSystem : IGuidanceSystem
    {
        public bool HasGuidance { get; protected set; }
        protected double ProportionalNavigationCoefficient { get; set; }
        protected Vector3D Position { get; set; }
        protected Vector3D Velocity { get; set; }
        protected Vector3D GuidanceCommand { get; set; }

        public BasicGuidanceSystem(double proportionalNavigationCoefficient)
        {
            ProportionalNavigationCoefficient = proportionalNavigationCoefficient;
            HasGuidance = false;
            GuidanceCommand = Vector3D.Zero;
            Position = Vector3D.Zero;
            Velocity = Vector3D.Zero;
        }

        public virtual void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
        {
            Position = missilePosition;
            Velocity = missileVelocity;            
        }

        public Vector3D GetGuidanceCommand()
        {
            return GuidanceCommand;
        }

        protected virtual void CalculateGuidanceCommand(double deltaTime)
        {
            // 基础制导系统不计算制导指令
            // 派生类应该重写这个方法来实现特定的制导逻辑
        }

        protected Vector3D CalculateProportionalNavigation(Vector3D missilePosition, Vector3D missileVelocity, Vector3D targetPosition, Vector3D targetVelocity)
        {
            // 预测时间（可以根据实际情况调整）
            double predictionTime = 2.0;

            // 预测目标位置
            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();
            
            // 添加调试输出
            Console.WriteLine($"PN Calculation: r = {r}, v = {v}, LOS = {LOS}, LOSRate = {LOSRate}, Acceleration = {acceleration}");
            
            return acceleration;
        }

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