SpaceEngeeneer_WHIPS_RocketGuidance_v11.cs

/*  
Space Engeeneer - Whip's Rocket Guidance Script v11 - revised: 10/28/15  
By Joshua Banks  
*/  
List<IMyTerminalBlock> remotes = new List<IMyTerminalBlock>();   
List<IMyTerminalBlock> shooterRefrenceList = new List<IMyTerminalBlock>();   
List<IMyTerminalBlock> missileRefrenceList = new List<IMyTerminalBlock>();   
List<IMyTerminalBlock> gyroList = new List<IMyTerminalBlock>();  
   
string shooterReferenceName = "[Shooter]";   
string missileReferenceName = "[Missile 2]";   
string gyroName = "[Control 2]";  
IMyRemoteControl shooterRefrence;   
IMyRemoteControl missileRefrence;   
bool hasRun = false;  
double pre_launch_delay = 2;  
double delta_origin;  
double max_rotation_degrees = 180; //in degrees per second (360 max for small ships, 180 max for large ships)  
double max_distance = 10000; //maximum guidance distance in meters    
int tick_limit = 1; //change to higher for less precision  
int current_tick = 0;   
int duration = 0;  
  
void Main(string argument)   
{    
    Echo("Tick: " + current_tick.ToString());  
  
    if (!hasRun)  
        GrabRemotes();  
      
    if((current_tick % tick_limit) == 0)   
    {   
        if (duration < Math.Ceiling(pre_launch_delay * 60) ) 
        {  
            duration++;  
            return;  
        }  
        else  
        {  
            Echo("Guidance Active");   
            GuideMissile();   
            current_tick = 0;   
        }      
    }   
    Echo("Has run?: " + hasRun.ToString());   
    current_tick++;   
}       
  
void GrabRemotes()  
{  
    GridTerminalSystem.SearchBlocksOfName(gyroName, gyroList);   
    GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(remotes);  
    for(int i = 0 ; i < remotes.Count ; i++)   
        {   
            var thisRemote = remotes[i] as IMyRemoteControl;  
            if(thisRemote.CustomName.Contains(shooterReferenceName))   
            {     
                shooterRefrenceList.Add(thisRemote as IMyRemoteControl);   
                Echo("Found Shooter");   
            }   
  
            if(thisRemote.CustomName.Contains(missileReferenceName))   
            {   
                missileRefrenceList.Add(thisRemote as IMyRemoteControl);   
                Echo("Found Missile");   
            }   
        }  
  
//---Check if we do not have an shooter remote  
    if(shooterRefrenceList.Count == 0)   
    {   
        Echo("No shooter refrence block found");   
        hasRun = false;   
        return;   
    }  
//---Check if we do not have a missile remote  
    else if(missileRefrenceList.Count == 0)   
    {   
        Echo("No missile refrence block found");   
        hasRun = false;   
        return;   
    }   
    else if(gyroList.Count == 0)  
    {  
        Echo("No control gyro found");  
        hasRun = false;  
        return;  
    }  
    else  
    {     
        Echo("Ready to run");  
        shooterRefrence = shooterRefrenceList[0] as IMyRemoteControl;   
        missileRefrence = missileRefrenceList[0] as IMyRemoteControl;   
        hasRun = true;   
    }  
}  
  
void GuideMissile()    
{    
//---Get positions of our blocks with relation to world center  
    var originPos = shooterRefrence.GetPosition();   
    var missilePos = missileRefrence.GetPosition();   
  
//---Find current distance from shooter to missile  
    delta_origin = Vector3D.Distance(originPos, missilePos);  
  
//---Check if we are in range      
    if(delta_origin < max_distance) //change this later to be larger  
    {  
    //---Get forward vector from our shooter vessel  
        var shooterForward = shooterRefrence.Position + Base6Directions.GetIntVector(shooterRefrence.Orientation.TransformDirection(Base6Directions.Direction.Forward));    
        var targetVector = shooterRefrence.CubeGrid.GridIntegerToWorld(shooterForward);    
        var targetVectorNorm = Vector3D.Normalize(targetVector - shooterRefrence.GetPosition());   
  
    //---Find vector from shooter to missile  
        var missileVector = Vector3D.Subtract(missilePos, originPos);  
  
    //---Calculate angle between shooter vector and missile vector  
        double dotProduct; Vector3D.Dot(ref targetVectorNorm, ref missileVector, out dotProduct);  
        double x = dotProduct / missileVector.Length();       
        double rawDevAngle = Math.Acos(x) * 180f / Math.PI; //angle between shooter vector and missile  
  
    //---Calculate perpendicular distance from shooter vector  
        var projectionVector = dotProduct * targetVectorNorm;  
        double deviationDistance = Vector3D.Distance(projectionVector,missileVector);  
        Echo("Angular Dev: " + rawDevAngle.ToString());  
  
    //---Determine scaling factor  
        double scalingFactor;  
        if(rawDevAngle < 90)  
        {  
            if(deviationDistance > 200)  
            {  
                scalingFactor = delta_origin; //if we are too far from the beam, dont add any more distance till we are closer  
            }  
            else  
            {  
                scalingFactor = (delta_origin + 200); //travel approx. 200m from current position in direction of target vector  
            }  
        }  
        else  
        {  
            scalingFactor = 200; //if missile is behind the shooter, goes 200m directly infront of shooter for better accuracy  
        }  
        var destination = shooterRefrence.GetPosition() + scalingFactor * targetVectorNorm;   
        Echo(destination.ToString()); //debug  
  
    //---Find front left and top vectors of our missileVector  
        var missileGridX = missileRefrence.Position + Base6Directions.GetIntVector(missileRefrence.Orientation.TransformDirection(Base6Directions.Direction.Forward));  
        var missileWorldX = missileRefrence.CubeGrid.GridIntegerToWorld(missileGridX) - missilePos;  
  
        var missileGridY = missileRefrence.Position + Base6Directions.GetIntVector(missileRefrence.Orientation.TransformDirection(Base6Directions.Direction.Left));  
        var missileWorldY = missileRefrence.CubeGrid.GridIntegerToWorld(missileGridY) - missilePos;  
  
        var missileGridZ = missileRefrence.Position + Base6Directions.GetIntVector(missileRefrence.Orientation.TransformDirection(Base6Directions.Direction.Up));  
        var missileWorldZ = missileRefrence.CubeGrid.GridIntegerToWorld(missileGridZ) - missilePos;  
  
    //---Find vector from missile to destination  
        var shipToTarget = Vector3D.Subtract(destination, missilePos);  
  
    //---Project target vector onto our top left and up vectors  
        double dotX; Vector3D.Dot(ref shipToTarget, ref missileWorldX, out dotX);  
        double dotY; Vector3D.Dot(ref shipToTarget, ref missileWorldY, out dotY);  
        double dotZ; Vector3D.Dot(ref shipToTarget, ref missileWorldZ, out dotZ);  
        var projTargetX = dotX / (missileWorldX.Length() * missileWorldX.Length()) * missileWorldX;  
        var projTargetY = dotY / (missileWorldY.Length() * missileWorldY.Length()) * missileWorldY;  
        var projTargetZ = dotZ / (missileWorldZ.Length() * missileWorldZ.Length()) * missileWorldZ;  
  
    //---Get Yaw and Pitch Angles  
        double angleYaw = Math.Atan(projTargetY.Length() / projTargetX.Length());  
        double anglePitch = Math.Atan(projTargetZ.Length() / projTargetX.Length());  
  
    //---Check if x is positive or negative  
        double checkPositiveX; Vector3D.Dot(ref missileWorldX, ref projTargetX, out checkPositiveX); Echo("check x:" + checkPositiveX.ToString());  
        if(checkPositiveX < 0)  
        {  
            angleYaw += Math.PI/2; //we only change one value so it doesnt spaz  
        }  
  
    //---Check if yaw angle is left or right  
        double checkYaw; Vector3D.Dot(ref missileWorldY, ref projTargetY, out checkYaw); Echo("check yaw:" + checkYaw.ToString());  
        if(checkYaw > 0) //yaw is backwards for what ever reason  
            angleYaw = -angleYaw;  
        Echo("yaw angle:" + angleYaw.ToString());  
  
    //---Check if pitch angle is up or down  
        double checkPitch; Vector3D.Dot(ref missileWorldZ, ref projTargetZ, out checkPitch); Echo("check pitch:" + checkPitch.ToString());  
        if(checkPitch < 0)  
            anglePitch = -anglePitch;  
        Echo("pitch angle:" + anglePitch.ToString());  
  
    //---Angle controller  
        double max_rotation_radians = max_rotation_degrees * (Math.PI / 180); 
        double yawSpeed = max_rotation_radians * angleYaw / Math.Abs(angleYaw); 
        double pitchSpeed = max_rotation_radians * anglePitch / Math.Abs(anglePitch); 
        /*if (angleYaw < Math.PI/4)  
        {  
            yawSpeed = angleYaw * max_rotation_radians / (Math.PI/4);  
        }  
        else  
        {  
            yawSpeed = max_rotation_radians;  
        }  
          
        if (anglePitch < Math.PI/4)  
        {  
            pitchSpeed = anglePitch * max_rotation_radians / (Math.PI/4);  
        }  
        else  
        {  
            pitchSpeed = max_rotation_radians;  
        }*/  
 
        //double yawSpeed = angleYaw / Math.PI * max_rotation_radians;  
        //double pitchSpeed = anglePitch / Math.PI * max_rotation_radians;  
        for(int i = 0; i < gyroList.Count; i++)  
        {  
            var thisGyro = gyroList[i] as IMyGyro;  
            thisGyro.SetValue<float>("Yaw", (float)yawSpeed);  
            thisGyro.SetValue<float>("Pitch", (float)pitchSpeed);  
            thisGyro.SetValue("Override", true);  
        }  
    }  
    else  
    {  
        Echo("Out of range");  
        for(int i = 0; i < gyroList.Count; i++)  
        {  
            var thisGyro = gyroList[i] as IMyGyro;  
            thisGyro.SetValue<float>("Yaw", 0f);  
            thisGyro.SetValue<float>("Pitch", 0f);  
            thisGyro.SetValue("Override", true);  
        }  
    }  
}