Super Simple Missile Code v1

/*
//Whip's Super Simple Missile Code for Rexxar v1 - 1/23/17

///SETUP///
You need at least:
* A timer set to run this program and trigger now itself
* A antenna
* A thruster pointed forward
* A remote
* A program
* A gyro

///FIRING INSTRUCTIONS///
Plug in a Vector3D or GPS coordinate into the argument and it will fly to that spot.
*/


//const string missileNameTag = "<Missile>";
//add this name tag to all missile blocks :)

//Me magical PID constants (idk if they will even work lol)
const double proportionalConstant = 5;
const double derivativeConstant = 2;

bool driftCompensation = true;
//set this to false if u want the missiles to drift like ass


// No touchey below
IMyRemoteControl missileReference = null;

bool fireMissile = false;
bool firstGuidance = true;

Vector3D targetPos = new Vector3D();
Vector3D missilePos = new Vector3D();
Vector3D gravVec = new Vector3D();

double lastYawAngle = 0;
double lastPitchAngle = 0;
double lastRollAngle = 0;

List<IMyThrust> thrust = new List<IMyThrust>();
List<IMyThrust> forwardThrust = new List<IMyThrust>();
List<IMyThrust> otherThrust = new List<IMyThrust>();
List<IMyGyro> gyros = new List<IMyGyro>();
List<IMyRemoteControl> remotes = new List<IMyRemoteControl>();
List<IMyRadioAntenna> antennas = new List<IMyRadioAntenna>();
List<IMyTimerBlock> timers = new List<IMyTimerBlock>();

void Main(string arg)
{
    var vector = new Vector3D();
    var gps = new Vector3D();


    bool isGPS = TryParseGPS(arg, out gps);

    if (isGPS)
    {
        Echo("GPS position recieved");
        targetPos = gps;
        if (!fireMissile)
            fireMissile = true;

        Echo(targetPos.ToString());
    }
    else
    {
        bool isVector = TryParseVector3D(arg, out vector);
        if (isVector)
        {
            Echo("Vector position recieved");
            targetPos = vector;
            if (!fireMissile)
                fireMissile = true;

            Echo(targetPos.ToString());
        }
    }
    if (fireMissile)
    {
        bool isSetup = GetBlocks();

        if (isSetup)
        {
            GuideMissile(targetPos);
            timers[0].ApplyAction("TriggerNow");
        }
    }
}

bool TryParseGPS(string gpsString, out Vector3D vector)
{
    vector = new Vector3D(0,0,0);

    var gpsStringSplit = gpsString.Split(':');

    double x, y, z;

    if (gpsStringSplit.Length < 5)
        return false;

    bool passX = double.TryParse(gpsStringSplit[2], out x);
    bool passY  = double.TryParse(gpsStringSplit[3], out y);
    bool passZ = double.TryParse(gpsStringSplit[4], out z);

    Echo($"{x},{y},{z}");

    if (passX && passY && passZ)
    {
        vector = new Vector3D(x, y, z);
        return true;
    }
    else
        return false;
}

bool TryParseVector3D(string vectorString, out Vector3D vector)
{
    vector = new Vector3D(0, 0, 0);

    vectorString = vectorString.Replace(" ", "").Replace("{", "").Replace("}", "").Replace("X", "").Replace("Y", "").Replace("Z", "");
    var vectorStringSplit = vectorString.Split(':');

    double x, y, z;

    if (vectorStringSplit.Length < 4)
        return false;

    bool passX = double.TryParse(vectorStringSplit[1], out x);
    bool passY = double.TryParse(vectorStringSplit[2], out y);
    bool passZ = double.TryParse(vectorStringSplit[3], out z);

    if (passX && passY && passZ)
    {
        vector = new Vector3D(x, y, z);
        return true;
    }
    else
        return false;
}

bool GetBlocks()
{
    bool successfulSetup = true;

    forwardThrust.Clear();
    otherThrust.Clear();

    GridTerminalSystem.GetBlocksOfType(gyros);
    GridTerminalSystem.GetBlocksOfType(thrust);
    GridTerminalSystem.GetBlocksOfType(remotes);
    GridTerminalSystem.GetBlocksOfType(antennas);
    GridTerminalSystem.GetBlocksOfType(timers);

    if (gyros.Count == 0)
    {
        Echo($"Error: No gyros");
        successfulSetup = false;
    }

    if (thrust.Count == 0)
    {
        Echo($"Error: No thrust");
        successfulSetup = false;
    }

    if (remotes.Count == 0)
    {
        Echo($"Error: No remotes");
        successfulSetup = false;
    }

    if (antennas.Count == 0)
    {
        Echo($"Error: No antenna");
        successfulSetup = false;
    }

    if (timers.Count == 0)
    {
        Echo($"Error: No timers");
        successfulSetup = false;
    }

    if (successfulSetup)
    {
        missileReference = remotes[0];
        GetGyroOrientation(missileReference, gyros);
        GetThrusterOrientation(missileReference, thrust, out forwardThrust, out otherThrust);


        var myID = Me.EntityId;
        foreach (IMyRadioAntenna thisAntenna in antennas)
        {
            thisAntenna.SetValue("PBList", myID);
            thisAntenna.SetValue("Radius", float.MaxValue);
            thisAntenna.SetValue("EnableBroadCast", true);
            thisAntenna.ApplyAction("OnOff_On");
        }

        foreach(IMyThrust thisThrust in otherThrust)
        {
            thisThrust.ApplyAction("OnOff_On");
        }

        foreach (IMyThrust thisThrust in forwardThrust)
        {
            thisThrust.ApplyAction("OnOff_On");
            thisThrust.SetValue("Override", float.MaxValue);
        }

    }

    return successfulSetup;
}

void GetThrusterOrientation(IMyRemoteControl refBlock, List<IMyThrust> thrusters, out List<IMyThrust> _forwardThrust, out List<IMyThrust> _otherThrust)
{
    var forwardDirn = refBlock.WorldMatrix.Forward;

    _forwardThrust = new List<IMyThrust>();
    _otherThrust = new List<IMyThrust>();

    foreach (IMyThrust thisThrust in thrusters)
    {
        var thrustDirn = thisThrust.WorldMatrix.Backward;
        bool sameDirn = thrustDirn == forwardDirn;

        if (sameDirn)
        {
            _forwardThrust.Add(thisThrust);
        }
        else
        {
            _otherThrust.Add(thisThrust);
        }
    }
}

void GuideMissile(Vector3D targetPos)
{
    missilePos = missileReference.GetPosition();

    //---Get orientation vectors of our missile
    Vector3D missileFrontVec = missileReference.WorldMatrix.Forward;
    Vector3D missileLeftVec = missileReference.WorldMatrix.Left;
    Vector3D missileUpVec = missileReference.WorldMatrix.Up;

    //---Check if we have gravity
    double rollAngle = 0; double rollSpeed = 0;

    var remote = remotes[0] as IMyRemoteControl;
    bool inGravity = false;

    gravVec = missileReference.GetNaturalGravity();
    double gravMagSquared = gravVec.LengthSquared();
    if (gravMagSquared != 0)
    {
        if (gravVec.Dot(missileUpVec) < 0)
        {
            rollAngle = Math.PI / 2 - Math.Acos(MathHelper.Clamp(gravVec.Dot(missileLeftVec) / gravVec.Length(), -1, 1));
        }
        else
        {
            rollAngle = Math.PI + Math.Acos(MathHelper.Clamp(gravVec.Dot(missileLeftVec) / gravVec.Length(), -1, 1));
        }

        if (firstGuidance) lastRollAngle = rollAngle;

        rollSpeed = Math.Round(proportionalConstant * rollAngle + derivativeConstant * (rollAngle - lastRollAngle) * 60, 2);

        inGravity = true;
    }
    else
    {
        rollSpeed = 0;
    }

    //---Find vector from missile to destinationVec
    var missileToTargetVec = targetPos - missilePos;                                                                                                                                                                                                                                                                                                     //w.H/i-P*L+a.s^H

    //---Get travel vector
    var missileVelocityVec = missileReference.GetShipVelocities().LinearVelocity;

    //---Calc our new heading based upon our travel vector

    var headingVec = CalculateHeadingVector(missileToTargetVec, missileVelocityVec, driftCompensation);

    //---Get pitch and yaw angles
    double yawAngle; double pitchAngle;
    GetRotationAngles(headingVec, missileFrontVec, missileLeftVec, missileUpVec, out yawAngle, out pitchAngle);

    if (firstGuidance)
    {
        lastPitchAngle = pitchAngle;
        lastYawAngle = yawAngle;
        firstGuidance = false;
    }

    //---Angle controller
    double yawSpeed = Math.Round(proportionalConstant * yawAngle + derivativeConstant * (yawAngle - lastYawAngle) * 60, 2);
    double pitchSpeed = Math.Round(proportionalConstant * pitchAngle + derivativeConstant * (pitchAngle - lastPitchAngle) * 60, 2);

    //---Set appropriate gyro override
    ApplyGyroOverride(pitchSpeed, yawSpeed, rollSpeed, gyros);

    //---Store previous values
    lastYawAngle = yawAngle;
    lastPitchAngle = pitchAngle;
    lastRollAngle = rollAngle;

}

Vector3D CalculateHeadingVector(Vector3D targetVec, Vector3D velocityVec, bool driftComp)
{
    if (!driftComp)
    {
        return targetVec;
    }

    if (velocityVec.LengthSquared() < 100)
    {
        return targetVec;
    }

    if (targetVec.Dot(velocityVec) > 0)
    {
        return VectorReflection(velocityVec, targetVec);
    }
    else
    {
        return -velocityVec;
    }
}

Vector3D VectorProjection(Vector3D a, Vector3D b) //proj a on b
{
    return a.Dot(b) / b.LengthSquared() * b;
}

Vector3D VectorReflection(Vector3D a, Vector3D b) //reflect a over b
{
    Vector3D project_a = VectorProjection(a, b);
    Vector3D reject_a = a - project_a;
    return project_a - reject_a;
}

double VectorAngleBetween(Vector3D a, Vector3D b) //returns radians
{
    if (a.LengthSquared() == 0 || b.LengthSquared() == 0)
        return 0;
    else
        return Math.Acos(MathHelper.Clamp(a.Dot(b) / a.Length() / b.Length(), -1, 1)); //avoids NAN errors
}

//Whip's Get Rotation Angles Method v4
void GetRotationAngles(Vector3D v_target, Vector3D v_front, Vector3D v_left, Vector3D v_up, out double yaw, out double pitch)
{
    //Dependencies: VectorProjection() | VectorAngleBetween()
    //Keen uses a stupid left hand rule coordSystem, I dont.
    var projTargetFront = VectorProjection(v_target, v_front);
    var projTargetLeft = VectorProjection(v_target, v_left);
    var projTargetUp = VectorProjection(v_target, v_up);
    var projTargetFrontLeft = projTargetFront + projTargetLeft;
    var projTargetFrontUp = projTargetFront + projTargetUp;

    yaw = VectorAngleBetween(v_front, projTargetFrontLeft);
    pitch = VectorAngleBetween(v_front, projTargetFrontUp);

    //---Check if yaw angle is left or right
    //multiplied by -1 to convert from right hand rule to left hand rule
    yaw = -1 * Math.Sign(v_left.Dot(projTargetLeft)) * yaw;

    //---Check if pitch angle is up or down
    pitch = Math.Sign(v_up.Dot(projTargetUp)) * pitch;

    //---Check if target vector is pointing opposite the front vector
    if (pitch == 0 && yaw == 0 && v_target.Dot(v_front) < 0)
    {
        yaw = Math.PI;
        pitch = Math.PI;
    }
}

//Whip's Gyro Orientation Method
string[] gyroRelativeYaw;
string[] gyroRelativePitch;
string[] gyroRelativeRoll;
int[] gyroYawSign;
int[] gyroPitchSign;
int[] gyroRollSign;

void GetGyroOrientation(IMyRemoteControl reference_block, List<IMyGyro> gyro_list)
{
    gyroRelativeYaw = new string[gyro_list.Count];
    gyroRelativePitch = new string[gyro_list.Count];
    gyroRelativeRoll = new string[gyro_list.Count];

    gyroYawSign = new int[gyro_list.Count];
    gyroPitchSign = new int[gyro_list.Count];
    gyroRollSign = new int[gyro_list.Count];

    var reference_up = reference_block.WorldMatrix.Up; //assuming rot right
    var reference_right = reference_block.WorldMatrix.Right; //assuming rot up
    var reference_forward = reference_block.WorldMatrix.Forward; //assuming rot up

    for (int i = 0; i < gyro_list.Count; i++)
    {
        var gyro_forward = gyro_list[i].WorldMatrix.Forward;
        var gyro_backward = gyro_list[i].WorldMatrix.Backward;
        var gyro_up = gyro_list[i].WorldMatrix.Up;
        var gyro_down = gyro_list[i].WorldMatrix.Down;
        var gyro_left = gyro_list[i].WorldMatrix.Left;
        var gyro_right = gyro_list[i].WorldMatrix.Right;

        /// Pitch Fields ///
        if (reference_right == gyro_forward)
        {
            gyroRelativePitch[i] = "Roll";
            gyroPitchSign[i] = 1;
        }
        else if (reference_right == gyro_backward)
        {
            gyroRelativePitch[i] = "Roll";
            gyroPitchSign[i] = -1;
        }
        else if (reference_right == gyro_right)
        {
            gyroRelativePitch[i] = "Pitch";
            gyroPitchSign[i] = 1;
        }
        else if (reference_right == gyro_left)
        {
            gyroRelativePitch[i] = "Pitch";
            gyroPitchSign[i] = -1;
        }
        else if (reference_right == gyro_up)
        {
            gyroRelativePitch[i] = "Yaw";
            gyroPitchSign[i] = -1;
        }
        else if (reference_right == gyro_down)
        {
            gyroRelativePitch[i] = "Yaw";
            gyroPitchSign[i] = 1;
        }

        /// Yaw Fields ///
        if (reference_up == gyro_forward)
        {
            gyroRelativeYaw[i] = "Roll";
            gyroYawSign[i] = -1;
        }
        else if (reference_up == gyro_backward)
        {
            gyroRelativeYaw[i] = "Roll";
            gyroYawSign[i] = 1;
        }
        else if (reference_up == gyro_right)
        {
            gyroRelativeYaw[i] = "Pitch";
            gyroYawSign[i] = -1;
        }
        else if (reference_up == gyro_left)
        {
            gyroRelativeYaw[i] = "Pitch";
            gyroYawSign[i] = 1;
        }
        else if (reference_up == gyro_up)
        {
            gyroRelativeYaw[i] = "Yaw";
            gyroYawSign[i] = 1;
        }
        else if (reference_up == gyro_down)
        {
            gyroRelativeYaw[i] = "Yaw";
            gyroYawSign[i] = -1;
        }

        /// Roll Fields ///
        if (reference_forward == gyro_forward)
        {
            gyroRelativeRoll[i] = "Roll";
            gyroRollSign[i] = 1;
        }
        else if (reference_forward == gyro_backward)
        {
            gyroRelativeRoll[i] = "Roll";
            gyroRollSign[i] = -1;
        }
        else if (reference_forward == gyro_right)
        {
            gyroRelativeRoll[i] = "Pitch";
            gyroRollSign[i] = 1;
        }
        else if (reference_forward == gyro_left)
        {
            gyroRelativeRoll[i] = "Pitch";
            gyroRollSign[i] = -1;
        }
        else if (reference_forward == gyro_up)
        {
            gyroRelativeRoll[i] = "Yaw";
            gyroRollSign[i] = -1;
        }
        else if (reference_forward == gyro_down)
        {
            gyroRelativeRoll[i] = "Yaw";
            gyroRollSign[i] = 1;
        }
    }
}

void ApplyGyroOverride(double pitch_speed, double yaw_speed, double roll_speed, List<IMyGyro> gyro_list)
{
    for (int i = 0; i < gyro_list.Count; i++)
    {
        var thisGyro = gyro_list[i];
        if (thisGyro != null)
        {
            thisGyro.SetValue<float>(gyroRelativeYaw[i], (float)yaw_speed * gyroYawSign[i]);
            thisGyro.SetValue<float>(gyroRelativePitch[i], (float)pitch_speed * gyroPitchSign[i]);
            thisGyro.SetValue<float>(gyroRelativeRoll[i], (float)roll_speed * gyroRollSign[i]);
            thisGyro.SetValue("Override", true);
        }
    }
}