【SE】导弹实现思路

其实是挺久以前弄的......

　　——序

要实现导弹，首先就是要制导。

所以，不能傻乎乎的直接撞过去，所以我们要设计一些算法，来让他直直的撞过去！

那么如何瞄准呢？SE提供了转向的设备（陀螺仪），相信聪明的读者已然想到！！！我们要通过陀螺仪转向，对准目标！！然后给他一炮！！！

......先等会，我们连如何控制转向都不知道呢(┬＿┬)......

接下来查阅资料，大佬的实验！！！https://tieba.baidu.com/p/6226176333?pn=2

知道了陀螺仪的三个参数分别是反向的yaw,pitch,roll轴的目标角速度(科学是挺科学的...可是为啥要反过来啊啊啊啊啊)

懂得了原理...接下来设计算法......

显然，只要得到我们与目标的相对角度，然后算一下我们的角速度，让陀螺仪只要不能刹车了就刹车，其余时间全部加速！！！

但是怎么获得相对角度呢......?

原来的想法是用逆三角函数根据XZ,YZ坐标求出相应的yaw,pitch轴，但是感觉挺麻烦而且不切实际（老觉得自己的想法不行）

于是查阅资料( 获得这篇神奇博客！！！https://www.kancloud.cn/kylilimin/mea-ship/1003204

里面的代码，告诉了我怎么获得相对角度，它先转换世界坐标系为局部坐标系(用法线变换，说实话这玩意我也不知道是啥，矩阵变换这玩意我也不是很懂）

然后将目标点在局部坐标系中分别投影到XZ,YZ！！！！

这样一来就很清晰明了，实际上与我的思路差不多，但是看起来更可靠一些(

然后用点积求平面上两向量(Z轴与投影到XZ,YZ上的标准向量)的夹角的cos(cos = |A · B| / |A|*|B| )，然后再用逆三角函数求得角......

是不是很科学......但是经过群里的大佬点拨，其实特么的直接用逆tan就可以了！！！傻了傻了(其实用点积比较有逼格)

这份代码中也有许多问题......不细说了，看下文。

解决了获取相对坐标的方式，就可以求解角速度了。

我们知道，v = Δv * t

则 t = v / Δv (1)

我们知道, t = s/v (2)

我们通过垃圾K社提供的垃圾API(    ，得到飞船的角速度v，和我们求出的相对角度s，通过这个公式t1 = s/v(2)，可以得到我们要走的时间！！！

我们又可以通过将所有陀螺仪开到最大，两帧相减算出其可提供的最大角加速度 Δv，注意，这个 Δv不是目标角加速度，只是陀螺仪一帧可以达到的最大角加速度，

所以实际上我们是不关心所谓的目标角加速度的，我们只关心一帧能有多少角加速度。

所以，刹车时间就是 t2 = v / Δv(1)，即可求得刹车时间。

所以，我们根据得到的t1和t2,显然可以得到如下关系

若t1 > t2，则时间足够刹车，可以尽情开动陀螺仪。

若t1 < t2，则时间不够刹车，需要尽力反向开动陀螺仪。

通过这两个关系的控制，我们可以控制时间是否足够刹车，也就完成了这个系统。（总感觉意犹未尽）

上面说的博客，提供了这种算法的特点：

1. 绝不会错过目标
2. 获得了抵达目标的时间最优解，即在不错过的情况下，用时最短
3. 精确无震荡
4. 存在动态误差，而且在圆周运动时候，动态误差会逐渐扩大到无法控制的程度

实际上，只要掌握了获取相对坐标的方法，同时记得请陀螺仪的加速是反的，其实也可以上个PID。

另外，需要注意的是，最好使用atan2这个函数，因为atan函数的值域只有-90~90,意味着只能处理两个象限的情况。

但是我们的目标可不会乖乖的呆在这两个象限，所以使用范围更大的atan2(-180~180)可以避免出现有两个位置拥有相同的相对角度导致不停往复。

推进器：直接无脑上个缓冲器，把Z轴(Forward)方向的缓冲控制去掉！！！Forward方向直接越级控制拉满！！！目标如果转移就是另外上下左右四个方向的推进器的事儿了！！！！我特么直接冲！！！

上个PID就完事儿了。

附代码(不完善，有些许bug，之后有时间再修改)

//by dudujerry
//5.28.导弹


//算法变量
double _AimTargetRatio = 1;//精度
List<Vector2D> _AimTarget_Data = new List<Vector2D>();
Vector2D _MaxAngleAcceleration;//最大角加速度,X=Yaw;y=Pitch
Vector2D _InitAngleVelocity;
int _BeforeAim = 60;//1秒出膛
int _TimeGoesBy = 0;

//设备变量
List<IMyGyro> _gyros = new List<IMyGyro>();
List<IMyRemoteControl> _remoteControls = new List<IMyRemoteControl>();
List<IMyLargeGatlingTurret> _gatlings = new List<IMyLargeGatlingTurret>();
IMyRemoteControl cp;
List<IMyThrust> _thrusts = new List<IMyThrust>();

//实际变量
MyDetectedEntityInfo _enemy = new MyDetectedEntityInfo();

void SetGyros(double yaw,double pitch){
    
    foreach(var gyro in _gyros){
        gyro.Enabled = true;
        gyro.GyroOverride = true;
        gyro.Yaw = (float)yaw;
        gyro.Pitch = (float)pitch;
    }
    
}

Vector3D SubVector3D(Vector3D a,Vector3D b){
    return new Vector3D(a.X - b.X,a.Y - b.Y, a.Z - b.Z);
    
}

bool InitGyros(){//测陀螺仪最大角加速度
    if(_MaxAngleAcceleration.Length() != 0){
        //若已经测过了，返回
        return true;
    }
    
    Vector3D angularVelocity = cp.GetShipVelocities().AngularVelocity;
    
    MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), cp.WorldMatrix.Forward, cp.WorldMatrix.Up);
    Vector3D meAngleVelocityToMe = Vector3D.TransformNormal(angularVelocity, refLookAtMatrix);
    //(*)-
    double meYawAngleVelocity = meAngleVelocityToMe.Y*180/Math.PI; //当前角速度
    double mePitchAngleVelocity = meAngleVelocityToMe.X*180/Math.PI;
    //得到当前帧的角速度(*)-
    
    if(_InitAngleVelocity.Length() == 0){
        _InitAngleVelocity = new Vector2D(meYawAngleVelocity,mePitchAngleVelocity);
        
        //陀螺仪全开
        //gys.SetOnOff(true);
        //gys.SetOverride(true);
        //gys.Yaw = gys.Pitch = 60;
        //(*)-
        SetGyros(60,60);
    }
    else{
        _MaxAngleAcceleration = new Vector2D((meYawAngleVelocity - _InitAngleVelocity.X)*60, (mePitchAngleVelocity - _InitAngleVelocity.Y)*60);
        //计算最大角加速度，由于一秒60帧所以乘60
        
        //gys.SetOverride(false);
        //gys.Yaw = gys.Pitch = 0;
        //返回原状态(*)-
        SetGyros(0,0);
    }
    return false;
}

void AimTarget(Vector3D targetPosition){
    double yaw = 0;//陀螺仪输出
    double pitch = 0;
    
    
    MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D() , cp.WorldMatrix.Forward, cp.WorldMatrix.Up);
    Vector3D positionToMe = Vector3D.TransformNormal(SubVector3D(targetPosition,cp.CenterOfMass),refLookAtMatrix);//[1]-cp.Position
    //转换坐标系 (*)-
    
    //Echo(positionToMe.ToString());
    double targetYawAngle = TargetAngleToMe(positionToMe,"Yaw");
    double targetPitchAngle = TargetAngleToMe(positionToMe,"Pitch");
    
    Echo("A:" + ((int)targetYawAngle).ToString() + " " + ((int)targetPitchAngle).ToString());
    
    //算出基于自己坐标系的相对角度
    
    while(_AimTarget_Data.Count > 30)//周期：30
    {
        _AimTarget_Data.RemoveAt(0);
    }
    while(_AimTarget_Data.Count < 30){
        _AimTarget_Data.Add(new Vector2D(targetYawAngle,targetPitchAngle));
    }
    _AimTarget_Data.RemoveAt(0);
    _AimTarget_Data.Add(new Vector2D(targetYawAngle,targetPitchAngle));
    //统计误差数据
    
    double meYawAngleVelocity = cp.GetShipVelocities().AngularVelocity.Y;
    double mePitchAngleVelocity = cp.GetShipVelocities().AngularVelocity.X;
    
    
    double effectYawVelocity = (_AimTarget_Data[_AimTarget_Data.Count - 1] .X - _AimTarget_Data[_AimTarget_Data.Count - 2] .X) * 60;
    double effectPitchVelocity = (_AimTarget_Data[_AimTarget_Data.Count - 1] .Y - _AimTarget_Data[_AimTarget_Data.Count - 2] .Y) * 60;
    Echo("合速度V:" + ((int)effectYawVelocity).ToString() + " " + ((int)effectPitchVelocity).ToString());
    //算合速度 (*)-
    
    if(effectYawVelocity*targetYawAngle > 0){
        //目标角度和合速度符号不同，即正朝着目标靠近
        if(Math.Abs(targetYawAngle) > _AimTargetRatio){
            //距离目标还有距离
            
            double stopTime = Math.Abs(effectYawVelocity / _MaxAngleAcceleration.X);
            //刹车时间
            double arriveTime = Math.Abs(targetYawAngle / effectYawVelocity);
            //抵达目标所用时间
        
            //yaw = -60 * (targetYawAngle >= 0 ? 1 : (-1));
        
        
            if(stopTime <= arriveTime){
                //刹车时间足够
                yaw = 60 * (targetYawAngle >= 0 ? 1 : (-1));
                //控制方向
                Echo("刹车时间足够:是");
            }
            else{
                yaw = -60 * (targetYawAngle >= 0 ? 1 : (-1));
                //控制方向
                Echo("刹车时间足够:否");
            }
        
        }
        else{
            //Echo("?:" + Math.Abs((int)targetYawAngle).ToString() +"-" + ((int)_AimTargetRatio).ToString());
            yaw = targetYawAngle*60/180;
            Echo("刹车时间足够:已到达");
        }
        
    }
    else {
            //正在远离目标
            yaw = 60 * (targetYawAngle >= 0 ? 1 : (-1));
            Echo("刹车时间足够:反方向");
    }
        
    if(effectPitchVelocity*targetPitchAngle > 0){
        //目标pitch速度和合速度符号不同，即正朝着目标靠近
        if(Math.Abs(targetPitchAngle) > _AimTargetRatio){
            //距离目标还有距离
            
            double stopTime = Math.Abs(effectPitchVelocity / _MaxAngleAcceleration.Y);
            //刹车时间
            double arriveTime = Math.Abs(targetPitchAngle / effectPitchVelocity);
            //抵达目标所用时间
        
            //pitch = -60 * (targetPitchAngle >= 0 ? 1 : (-1));
        
            if(stopTime <= arriveTime){
                //刹车时间足够
                Echo("刹车时间足够:是");
                pitch = -60 * (targetPitchAngle >= 0 ? 1 : (-1));
                //控制方向
            }
            else{
                pitch = 60 * (targetPitchAngle >= 0 ? 1 : (-1));
                Echo("刹车时间足够:否");
                //控制方向
            }
        
        }
        else{
            pitch = -targetPitchAngle*60/180;
            Echo("刹车时间足够:已到达");

        }
        
    }
    else {
            //正在远离目标
            pitch = -60 * (targetPitchAngle >= 0 ? 1 : (-1));
            Echo("刹车时间足够:反方向");
            
        }
    Echo("Yaw:" + yaw.ToString() + " Pitch:" + pitch.ToString());
    
    //gys.SetOverride(true);
    //gys.Yaw = yaw;
    //gys.Pitch = pitch;
    //控制陀螺仪(*)-
    SetGyros(yaw,pitch);
    
    Echo("自己的V:" + ((int)meYawAngleVelocity).ToString()+((int)mePitchAngleVelocity).ToString());
    Echo("陀螺仪的V:" + ((int)_gyros[0].Yaw).ToString()+((int)_gyros[0].Pitch).ToString());
}

public static double TargetAngleToMe(Vector3D targetToMe,string direction){
    targetToMe = Vector3D.Normalize(targetToMe);
    Vector3D zForward = new Vector3D(0,0,-1);
    if(direction == "Yaw"){
        
        Vector3D targetYawVector = new Vector3D(targetToMe.X,0,targetToMe.Z);
        
        return Math.Atan2(targetToMe.X,targetToMe.Z) * 180/Math.PI;
        
    }
    else if (direction == "Pitch"){
        Vector3D targetYawVector = new Vector3D(0,targetToMe.Y,targetToMe.Z);
        return Math.Atan2(targetToMe.Y,targetToMe.Z) * 180/Math.PI;
        
        
    }
    return 0;
}




public Program(){
    Runtime.UpdateFrequency = UpdateFrequency.Update1;
    
    GridTerminalSystem.GetBlocksOfType<IMyGyro>(_gyros);
    GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(_remoteControls);
    GridTerminalSystem.GetBlocksOfType<IMyLargeGatlingTurret>(_gatlings);
    GridTerminalSystem.GetBlocksOfType<IMyThrust>(_thrusts);
    
    cp = _remoteControls[0];
    Echo("Programed");
    
}

public void ControlThrusts(){
    
    MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D() , cp.WorldMatrix.Forward, cp.WorldMatrix.Up);
    
    foreach(var thrust in _thrusts){
        var maxThrust = thrust.MaxEffectiveThrust;
        
        Echo("in");
        
        switch (cp.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward)){
            case Base6Directions.Direction.Forward:
                thrust.ThrustOverride = maxThrust;
                thrust.ThrustOverridePercentage = 1;
                
            break;
            
            case Base6Directions.Direction.Right:
            
                float velocity1 = (float)-(Vector3D.TransformNormal(cp.GetShipVelocities().LinearVelocity,refLookAtMatrix)).X;
                thrust.ThrustOverridePercentage = (velocity1 > 0) ? velocity1 : 0; 
            break;
             
            case Base6Directions.Direction.Left:
                float velocity2 = (float)-(Vector3D.TransformNormal(cp.GetShipVelocities().LinearVelocity,refLookAtMatrix)).X;
                thrust.ThrustOverridePercentage = (velocity2 < 0) ? -velocity2 : 0; 
            break;
            
            case Base6Directions.Direction.Up:
                float velocity3 = (float)-(Vector3D.TransformNormal(cp.GetShipVelocities().LinearVelocity,refLookAtMatrix)).Y;
                thrust.ThrustOverridePercentage = (velocity3 > 0) ? velocity3 : 0; 
            break;
             
            case Base6Directions.Direction.Down:
                float velocity4 = (float)-(Vector3D.TransformNormal(cp.GetShipVelocities().LinearVelocity,refLookAtMatrix)).Y;
                thrust.ThrustOverridePercentage = (velocity4 < 0) ? -velocity4 : 0; 
            break;
        }
    }
    
}


public void Main(string argument, UpdateType updateSource)
{
    if(_TimeGoesBy <= _BeforeAim)
        _TimeGoesBy ++;
    if(InitGyros() == false)
        return;
    
    MyDetectedEntityInfo enemy = _gatlings[0].GetTargetedEntity();
    //Echo(enemy.Position.ToString());
    if(enemy.IsEmpty() == false){
        _enemy = enemy;
    }
    
    //Echo( "rel:" + _enemy.Relationship.Enemies + _enemy.Relationship.Neutral);
    //&& (_enemy.Type == MyDetectedEntityType.SmallGrid || _enemy.Type == MyDetectedEntityType.LargeGrid)
    
    
    if(_enemy.IsEmpty() == false && _TimeGoesBy > _BeforeAim && (_enemy.Relationship == MyRelationsBetweenPlayerAndBlock.Enemies || _enemy.Relationship == MyRelationsBetweenPlayerAndBlock.Neutral) ){
        AimTarget(_enemy.Position);
        Echo("Aimed");
    }
    else if (_enemy.IsEmpty() == true)
    {
        Echo("Can't Find Entity");
    }
    //Echo(TargetAngleToMe(new Vector3D(4,2,2),"Yaw").ToString());
    //ControlThrusts();
    
    
    
    return;
}

二营长，你他娘的意大利炮呢？给我拉上来！