Whip's GASP Missile Script v37.4c

/*
Whip's GASP Missile System v37.4c - revised: 2/27/16
/// PERSONAL VERSION ///
/// Stable ///
    Missile 1
___________________________________________________________________
Description:
    This script allows the user to manually guide a missile with the
    heading of his/her ship.

To-do:
    -account for landing gear rotors and connectors
    -account for reactors as power source

Changes:
    -changed dT from ideal to measured
    -added PD to roll
    -added CoM offset
    -alysius' drift concept

Code by Whiplash141 :)
*/

/*
___________________________________________________________________

========== You can edit these variables to your liking ============
___________________________________________________________________
*/

//---Missile name
    const string missileNumber = "1";
    const string strMissileTag = "Missile " + missileNumber; //(MANDATORY) unique id of the missile
    const string strMainThrustTag = "Main"; //(MANDATORY) tag on main forward thrusters
    const string strSideThrustTag = "Side"; //(Optional) tag on side thrusters
    const string strDetachThrustTag = "Detach"; //(Optional) tag on detach thrust

//---Reference name
    const string strShooterReferenceName = "Shooter Reference"; //name of the remote on the shooter vessel

//---Runtime vars
    const int updates_per_second = 10; //self explanatory :P
    const double battery_discharge_duration = 1;
    const double guidance_delay = 1; //time (in seconds) that the missile will delay guidance activation by
    const double detach_duration = 1; //time that the missile will execute detaching function
    const double main_ignition_delay =  1; //time (in seconds) that the missile will delay main engine activation

//---Control system
    const double proportionalConstant = 100;
    const double derivativeConstant = 50;

//---Drift compensation
    static bool driftCompensation = true;
    const double compensationConstant = 2;
    const double centerOfMassOffset = -1.5 * .5; //in meters measured from the control gyro
/*
___________________________________________________________________

============= Don't touch anything below this :) ==================
___________________________________________________________________
*/
//---So many lists...
    List<IMyTerminalBlock> missileBlocks = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> mainThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> sideThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> detachThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> artMasses = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> mergeBlocks = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> batteries = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> remotes = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> shooterRefrenceList = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> gyros = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> timers = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> programs = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> importantBlocks = new List<IMyTerminalBlock>();

    Vector3D shooterFrontNorm;
    Vector3D shooterLeftNorm;
    Vector3D shooterUpNorm;
    Vector3D originPos;
    Vector3D missilePos;
    Vector3D lastMissilePos;
    Vector3D headingVec;
    Vector3D destinationVec;
    Vector3D gravVec;

    IMyRemoteControl shooterRefrence;
    IMyGyro missileRefrence;

    bool isSetup = false;
    bool firstRun = true;
    bool shouldKill = false;
    bool firstGuidance = true;
    bool hasPassed = false;
    bool killAllowed = false;
    bool inGravity = false;
    bool controlGyros = true;

    bool missileStage1 = false;
    bool missileStage2 = false;
    bool missileStage3 = false;
    bool missileStage4 = false;


    double distanceFromShooter;
    double max_kill_time = 3;
    double kill_time = 0;
    double timeElapsed = 0; //time elapsed over current iterations
    double timeTotal = 0; //total time program has been running
    double lastYawAngle = 0;
    double lastPitchAngle = 0;
    double lastRollAngle = 0;

    static double max_distance = 10000; //static b/c we change it on kill command
    const double degToRad = Math.PI / 180;
    const double radToDeg = 180 / Math.PI;
    const double max_time_to_guide = 300; //in seconds
    const double timeLimit =  1 / (double)updates_per_second;
    double compensationBounds = Math.Acos(1 / compensationConstant) * radToDeg;

void Main( string arg )
{
    if( arg.ToLower() == "run" )
    {
        if (!isSetup)
        {
            GrabBlocks();
        }
        else //will not run or release missile until has run setup succesfully
        {

            if( firstRun )
            {
                timeElapsed = 0;
                timeTotal = 0;
                firstRun = false;
            }else{
                timeElapsed += ElapsedTime.TotalSeconds;
                timeTotal += ElapsedTime.TotalSeconds;
            }

            LaunchMissile();
            StatusCheck();
            if( timeTotal > guidance_delay + battery_discharge_duration && timeElapsed >= timeLimit)
            {
                Echo("Guidance Active"); Echo("Run Time: " + Math.Round(timeTotal).ToString());
                GuideMissile();
                timeElapsed = 0;
            }
        }
    }
    else if(arg.ToLower() == "setup")
    {
        GrabBlocks();
    }
    else if(arg.ToLower() == "kill" && killAllowed)
    {
        shouldKill = true;
        max_distance = double.PositiveInfinity;
    }

    if(timeTotal > max_time_to_guide)
    {
        shouldKill = true;
        max_distance = double.PositiveInfinity;
    }
}

void GrabBlocks()
{
    GridTerminalSystem.SearchBlocksOfName( strMissileTag, missileBlocks );
    GridTerminalSystem.SearchBlocksOfName( strShooterReferenceName, shooterRefrenceList );

    for( int i = 0; i < shooterRefrenceList.Count; i++ )
    {
        importantBlocks.Add( shooterRefrenceList[i] );
    }

    for( int i = 0; i < missileBlocks.Count; i++ )
    {
        var thisBlock = missileBlocks[i] as IMyTerminalBlock;

        if( thisBlock is IMyThrust )
        {
            if( thisBlock.CustomName.Contains( strSideThrustTag ) &&
                thisBlock.CustomName.Contains( strDetachThrustTag ) )
            {
                sideThrusters.Add( thisBlock );
                detachThrusters.Add( thisBlock );
            }
            else if( thisBlock.CustomName.Contains( strMainThrustTag ) &&
                thisBlock.CustomName.Contains( strDetachThrustTag ) )
            {
                mainThrusters.Add( thisBlock );
                detachThrusters.Add( thisBlock );
            }
            else if( thisBlock.CustomName.Contains( strSideThrustTag ) )
            {
                sideThrusters.Add( thisBlock );
            }
            else if( thisBlock.CustomName.Contains( strMainThrustTag ) )
            {
                mainThrusters.Add( thisBlock );
            }
            else if( thisBlock.CustomName.Contains ( strDetachThrustTag ) )
            {
                detachThrusters.Add( thisBlock );
            }
        }
        else if( thisBlock is IMyVirtualMass )
        {
            artMasses.Add( thisBlock );
        }
        else if( thisBlock is IMyBatteryBlock )
        {
            batteries.Add( thisBlock );
        }
        else if( thisBlock is IMyGyro )
        {
            gyros.Add( thisBlock );
            importantBlocks.Add( thisBlock );
        }
        else if( thisBlock is IMyShipMergeBlock )
        {
            mergeBlocks.Add( thisBlock );
        }
        else if( thisBlock is IMyTimerBlock )
        {
            timers.Add( thisBlock );
            importantBlocks.Add( thisBlock );
        }
        else if( thisBlock is IMyProgrammableBlock )
        {
            programs.Add( thisBlock );
            importantBlocks.Add( thisBlock );
        }
        else if( thisBlock is IMyRemoteControl )
        {
            remotes.Add( thisBlock );
        }
    }

    if( sideThrusters.Count == 0 )
    {
        Echo("[OPTIONAL] No side thrusters found");
    }

    if( detachThrusters.Count == 0 )
    {
        Echo("[OPTIONAL] No detach thrusters found");
    }

    if( artMasses.Count == 0 )
    {
        Echo("[OPTIONAL] No artificial masses found");
    }

    if( shooterRefrenceList.Count == 0 )
    {
        Echo("[FAILED] No shooter refrence block found");
        isSetup = false;
        return;
    }
    else if( gyros.Count == 0 )
    {
        Echo("[FAILED] No control gyros found");
        isSetup = false;
        return;
    }
    else if( mainThrusters.Count == 0)
    {
        Echo("[FAILED] No main thrusters found");
        isSetup = false;
        return;
    }
    else if( batteries.Count == 0)
    {
        Echo("[FAILED] No batteries found");
        isSetup = false;
        return;
    }
    else if( mergeBlocks.Count == 0)
    {
        Echo("[FAILED] No merge blocks found");
        isSetup = false;
        return;
    }
    else
    {
        Echo("[SUCCESS] Ready to run");
        shooterRefrence = shooterRefrenceList[0] as IMyRemoteControl;
        missileRefrence = gyros[0] as IMyGyro;
        isSetup = true;
    }
}

void StatusCheck()
{
    for (int k = 0; k < importantBlocks.Count; k++)
    {
        IMyTerminalBlock block = importantBlocks[k];
        IMySlimBlock slim = block.CubeGrid.GetCubeBlock(block.Position);
        if( slim.CurrentDamage > 0 )
        {
            Echo("Damage");
            kill_time = max_kill_time;
            KillGuidance( 0, 0 );
            return;
        }
    }
}

void LaunchMissile()
{
//stage 1
    if ( !missileStage1 ) //set battery to discharge
    {
        for(int i = 0 ; i < batteries.Count ; i++)
        {
            var thisBattery = batteries[i] as IMyBatteryBlock;
            if( thisBattery != null )
            {
                thisBattery.ApplyAction( "OnOff_On" ); //make sure our battery is on
                thisBattery.SetValue( "Recharge", false );
            }
        }
        missileStage1 = true;
    }
//stage 2
    else if( timeTotal >= battery_discharge_duration && !missileStage2 ) //detach missile
    {
        for( int i = 0 ; i < artMasses.Count ; i++ )
        {
            var thisMass = artMasses[i] as IMyVirtualMass;
            if( thisMass != null ) thisMass.ApplyAction( "OnOff_On" );
        }

        for( int i = 0; i < gyros.Count; i++ )
        {
            var thisGyro = gyros[i] as IMyGyro;
            if( thisGyro != null ) thisGyro.ApplyAction("OnOff_On");
        }

        for( int i = 0 ; i < mergeBlocks.Count ; i++ )
        {
            var thisMerge = mergeBlocks[i] as IMyShipMergeBlock;
            if( thisMerge != null ) thisMerge.ApplyAction( "OnOff_Off" );
        }

        for( int i = 0; i < detachThrusters.Count; i++ )
        {
            var thisThrust = detachThrusters[i] as IMyThrust;
            if( thisThrust != null )
            {
                thisThrust.ApplyAction( "OnOff_On" );
                thisThrust.SetValue<float>( "Override", float.MaxValue );
            }
        }

        ManeuveringThrust( false );
        killAllowed = true;
        missileStage2 = true;

    }
//stage 3
    else if( timeTotal >= battery_discharge_duration + detach_duration && !missileStage3 )
    {
        ManeuveringThrust( true );

        for( int i = 0; i < detachThrusters.Count; i++ )
        {
            var thisThrust = detachThrusters[i] as IMyThrust;
            if( thisThrust != null ) thisThrust.SetValue<float>( "Override", float.MinValue );
        }
        missileStage3 = true;
    }
//stage 4
    else if( timeTotal >= battery_discharge_duration + main_ignition_delay + detach_duration && !missileStage4) //fire missile
    {
        for( int i = 0 ; i < artMasses.Count ; i++ )
        {
            var thisMass = artMasses[i] as IMyVirtualMass;
            if( thisMass != null ) thisMass.ApplyAction( "OnOff_Off" );
        }

        for( int i = 0 ; i < mergeBlocks.Count ; i++ ) //for safety
        {
            var thisMerge = mergeBlocks[i] as IMyShipMergeBlock;
            if( thisMerge != null && thisMerge.GetValue<bool>("OnOff") ) thisMerge.ApplyAction( "OnOff_Off" );
        }

        MainThrustOverride();
        missileStage4 = true;
    }
}

void GuideMissile()
{
//---Get positions of our blocks with relation to world center
    if( !shouldKill )
        originPos = shooterRefrence.GetPosition();

    missilePos = missileRefrence.GetPosition();

//---Find current distance from shooter to missile
    distanceFromShooter = Vector3D.Distance( originPos, missilePos );

//---Check if we are in range
    if( distanceFromShooter > max_distance )
    {
        Echo( "Out of range" );
        shouldKill = true;
    }

//---Get orientation vectors from our shooter vessel
    if( !shouldKill )
    {
        MatrixD shoterOrientation = shooterRefrence.WorldMatrix;
        Vector3D shooterForwardVec = shoterOrientation.Forward;
        Vector3D shooterLeftVec = shoterOrientation.Left;
        Vector3D shooterUpVec = shoterOrientation.Up;

        shooterFrontNorm = Vector3D.Normalize( shooterForwardVec );
        shooterLeftNorm = Vector3D.Normalize( shooterLeftVec );
        shooterUpNorm = Vector3D.Normalize( shooterUpVec );
    }

//---Find vector from shooter to missile
    var shooterToMissile = missilePos - originPos;

//---Determine where missile is in relation to shooter
    bool isLeft = CheckDotPositive( shooterToMissile, shooterLeftNorm );
    bool isUp = CheckDotPositive( shooterToMissile, shooterUpNorm );
    int signLeft; int signUp;
    if( isLeft )
        signLeft = 1;
    else
        signLeft = -1;

    if( isUp )
        signUp = 1;
    else
        signUp = -1;

//---Calculate angle between shooter vector and missile vector
    double rawDevAngle = Math.Acos( shooterFrontNorm.Dot( Vector3D.Normalize( shooterToMissile ) ) ) * radToDeg;

//---Calculate perpendicular distance from shooter vector
    var projectionVec = VectorProjection( shooterToMissile, shooterFrontNorm );
    double deviationDistance = Vector3D.Distance( projectionVec, shooterToMissile );

//---Find front left and top vectors of our missile
    MatrixD missileOrientation = missileRefrence.WorldMatrix;
    Vector3D missileFrontVec = missileOrientation.Forward;
    Vector3D missileLeftVec = missileOrientation.Left;
    Vector3D missileUpVec = missileOrientation.Up;

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

    if( remotes.Count != 0 )
    {
        var remote = remotes[0] as IMyRemoteControl;
        inGravity = false;
        if( remote != null )
        {
            gravVec = remote.GetNaturalGravity();
            double gravMag = gravVec.Length();
            if( !double.IsNaN( gravMag ) && gravMag != 0 )
            {
                if( gravVec.Dot( missileUpVec ) < 0 )
                {
                    rollAngle = Math.PI / 2 - Math.Acos( gravVec.Dot( missileLeftVec ) / gravVec.Length() / missileLeftVec.Length() );
                }else{
                    rollAngle = Math.PI + Math.Acos( gravVec.Dot( missileLeftVec ) / gravVec.Length() / missileLeftVec.Length() );
                }

                if(firstGuidance) lastRollAngle = rollAngle;

                rollSpeed = proportionalConstant * rollAngle + derivativeConstant * (rollAngle - lastRollAngle) / timeElapsed;

                inGravity = true;
            }
        }
    }

//---Determine scaling factor
    double scalingFactor;
    if( rawDevAngle < 90 )
    {
        scalingFactor = projectionVec.Length() + 200; //travel approx. 250m from current position in direction of target vector
        destinationVec = shooterRefrence.GetPosition() + scalingFactor * shooterFrontNorm;
        if( !hasPassed )
            hasPassed = true;
    }
    else if( hasPassed )
    {
        scalingFactor = -projectionVec.Length() + 200;
        destinationVec = shooterRefrence.GetPosition() + scalingFactor * shooterFrontNorm + signLeft * 50 * shooterLeftNorm + signUp * 50 * shooterUpNorm;
    }
    else
    {
        scalingFactor = -projectionVec.Length() + 200;
        destinationVec = shooterRefrence.GetPosition() + scalingFactor * shooterFrontNorm;
    }

//---Find vector from missile to destinationVec
    var missileToTargetVec = Vector3D.Subtract( destinationVec, missilePos );

//---Check if we have any initial values
    if(firstGuidance)
    {
        lastMissilePos = missilePos;
    }


//---Offset the missile position for the Comparer
    missilePos += centerOfMassOffset * missileFrontVec;


//---Get travel vector
    var travelVec = missilePos - lastMissilePos;

//---Calc our new heading based upon our travel vector
    //headingVec = missileToTargetVec;
    if(missileStage4)
    {
        headingVec = CalculateHeadingVector(missileToTargetVec, travelVec, driftCompensation);
    }else{
        headingVec = CalculateHeadingVector(missileToTargetVec, travelVec, false);
    }

//---Project target vector onto our top left and up vectors
    var projTargetFront = VectorProjection( headingVec, missileFrontVec);
    var projTargetLeft = VectorProjection( headingVec, missileLeftVec);
    var projTargetUp = VectorProjection( headingVec, missileUpVec);
    var projTargetFrontLeftPlane = projTargetFront + projTargetLeft;

//---Get Yaw and Pitch Angles
    double yawAngle = Math.Atan( projTargetLeft.Length() / projTargetFront.Length() );
    double pitchAngle = Math.Atan( projTargetUp.Length() / projTargetFrontLeftPlane.Length() );

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

//---Check if x is positive or negative
    bool isPositiveFront = CheckDotPositive( missileFrontVec, projTargetFront );
    if( !isPositiveFront )
        yawAngle += Math.PI/2; //we only change one value so it doesnt spaz

//---Check if yaw angle is left or right
    bool isPositiveLeft = CheckDotPositive( missileLeftVec, projTargetLeft );
    if( isPositiveLeft ) //yaw is backwards for what ever reason
        yawAngle = -yawAngle;

//---Check if pitch angle is up or down
    bool isPositiveUp = CheckDotPositive( missileUpVec, projTargetUp );
    if( !isPositiveUp )
        pitchAngle = -pitchAngle;

//---Angle controller
    double yawSpeed = proportionalConstant * yawAngle + derivativeConstant * (yawAngle - lastYawAngle) / timeElapsed;
    double pitchSpeed = proportionalConstant * pitchAngle + derivativeConstant * (pitchAngle - lastPitchAngle) / timeElapsed;

//---Set appropriate gyro override
    if( controlGyros )
    {
        for( int i = 0; i < gyros.Count; i++ )
        {
            var thisGyro = gyros[i] as IMyGyro;
            if(thisGyro != null)
            {
                thisGyro.SetValue<float>( "Yaw", (float)yawSpeed );
                thisGyro.SetValue<float>( "Pitch", (float)pitchSpeed );
                if( inGravity )
                {
                    thisGyro.SetValue<float>( "Roll", (float)rollSpeed );
                }
                thisGyro.SetValue( "Override", true );
            }
        }
    }

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

    if( shouldKill )
    {
        KillGuidance( rawDevAngle, deviationDistance );
    }
}

Vector3D CalculateHeadingVector( Vector3D target, Vector3D travel, bool driftComp )
{
    var targetNorm = Vector3D.Normalize(target);
    if(!driftComp)
    {
        return targetNorm;
    }
    var travelNorm = Vector3D.Normalize(travel);
    //var projTemp = VectorProjection( travelNorm, targetNorm );
    //var rejTemp =  projTemp - travelNorm;
    if(travel.Length() / timeLimit < 10)
    {
        return targetNorm;
    }else{
        return  targetNorm - (travelNorm / compensationConstant);
    }
}

void ManeuveringThrust( bool turnOn )
{
    for( int i = 0 ; i < sideThrusters.Count ; i++ )
    {
        IMyThrust thisThrust = sideThrusters[i] as IMyThrust;
        if(thisThrust != null)
        {
            if( turnOn ) { thisThrust.ApplyAction( "OnOff_On" ); }
            else { thisThrust.ApplyAction( "OnOff_Off" ); }
        }
    }
}

void MainThrustOverride()
{
    for( int i = 0; i < mainThrusters.Count; i++ )
    {
        IMyThrust thisThrust = mainThrusters[i] as IMyThrust;
        if(thisThrust != null)
        {
            thisThrust.ApplyAction( "OnOff_On" );
            thisThrust.SetValue<float>( "Override", float.MaxValue );
        }
    }
}

void KillGuidance( double angleOfDeviation, double distanceOfDeviation )
{
    if( angleOfDeviation < 5 && distanceOfDeviation < 1 )
    {
        kill_time += timeElapsed;
    }else{
        kill_time = 0;
    }

    if( kill_time >= max_kill_time || timeTotal >= max_time_to_guide )
    {
        for( int i = 0; i < gyros.Count; i++ )
        {
            var thisGyro = gyros[i] as IMyGyro;
            if(thisGyro != null)
            {
                thisGyro.SetValue<float>( "Yaw", 0f );
                thisGyro.SetValue<float>( "Pitch", 0f );
                thisGyro.SetValue<float>( "Roll", 0f );
                thisGyro.SetValue( "Override", true );
            }
        }

        if( !inGravity )
        {
            for( int i = 0; i < mainThrusters.Count; i++ )
            {
                var thisThruster = mainThrusters[i] as IMyThrust;
                if(thisThruster != null) thisThruster.ApplyAction( "OnOff_Off" );
            }

            for( int i = 0; i < sideThrusters.Count; i++ )
            {
                var thisThruster = sideThrusters[i] as IMyThrust;
                if(thisThruster != null) thisThruster.ApplyAction( "OnOff_Off" );
            }
        }

        for( int i = 0; i < importantBlocks.Count; i++ )
        {
            var thisBlock = importantBlocks[i] as IMyTerminalBlock;
            if(thisBlock != null)
            {
                if( !( thisBlock is IMyRemoteControl ) || !( thisBlock is IMyProgrammableBlock ) )
                    thisBlock.ApplyAction( "OnOff_Off" );
            }
        }
    }
}

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

bool CheckDotPositive( Vector3D a, Vector3D b )
{
    double check =  a.Dot( b );
    if( check > 0 )
    {
        return true;
    }else{
        return false;
    }
}