//Standard Guidance Routine
#region RdavNav Missile Guidance #RFC#
/*=================================================
* RdavNav
* --------------------------------------- */
void STD_GUIDANCE(MISSILE This_Missile)
{
//Targeting Module
//-----------------------------------------------
//Retrieves Target Position
var This_Missile_Director = This_Missile.TURRET as IMyLargeTurretBase;
var ENEMY_POS = new Vector3D();
//Logical Determination Of Enemy Position
if (This_Missile_Director.GetTargetedEntity().IsEmpty() == false)
{
ENEMY_POS = This_Missile_Director.GetTargetedEntity().Position; //Also based on position for critical hits
}
//else if (!(This_Missile.TARGET_PREV_POS == null)) //new Vector3D()
//{ENEMY_POS = This_Missile.TARGET_PREV_POS;}
else
{
ENEMY_POS = RC.GetPosition() + RC.WorldMatrix.Forward * ((This_Missile.GYRO.GetPosition() - Me.GetPosition()).Length() + 300);
}
//Sorts CurrentVelocities
Vector3D MissilePosition = This_Missile.GYRO.CubeGrid.WorldVolume.Center;
Vector3D MissilePositionPrev = This_Missile.MIS_PREV_POS;
Vector3D MissileVelocity = (MissilePosition - MissilePositionPrev) / Global_Timestep;
Vector3D TargetPosition = ENEMY_POS;
Vector3D TargetPositionPrev = This_Missile.TARGET_PREV_POS;
Vector3D TargetVelocity = (TargetPosition - This_Missile.TARGET_PREV_POS) / Global_Timestep;
//Uses RdavNav Navigation APN Guidance System
//-----------------------------------------------
//Setup LOS rates and PN system
Vector3D LOS_Old = Vector3D.Normalize(TargetPositionPrev - MissilePositionPrev);
Vector3D LOS_New = Vector3D.Normalize(TargetPosition - MissilePosition);
Vector3D Rel_Vel = Vector3D.Normalize(TargetVelocity - MissileVelocity);
//And Assigners
Vector3D am = new Vector3D(1, 0, 0); double LOS_Rate; Vector3D LOS_Delta;
Vector3D MissileForwards = This_Missile.THRUSTERS[0].WorldMatrix.Backward;
//Vector/Rotation Rates
if (LOS_Old.Length() == 0)
{
LOS_Delta = new Vector3D(0, 0, 0); LOS_Rate = 0.0;
}
else
{
LOS_Delta = LOS_New - LOS_Old; LOS_Rate = LOS_Delta.Length() / Global_Timestep;
}
//-----------------------------------------------
//Closing Velocity
double Vclosing = (TargetVelocity - MissileVelocity).Length();
//If Under Gravity Use Gravitational Accel
Vector3D GravityComp = -RC.GetNaturalGravity();
//Calculate the final lateral acceleration
Vector3D LateralDirection = Vector3D.Normalize(Vector3D.Cross(Vector3D.Cross(Rel_Vel, LOS_New), Rel_Vel));
Vector3D LateralAccelerationComponent = LateralDirection * PNGain * LOS_Rate * Vclosing + LOS_Delta * 9.8 * (0.5 * PNGain); //Eases Onto Target Collision LOS_Delta * 9.8 * (0.5 * Gain)
//If Impossible Solution (ie maxes turn rate) Use Drift Cancelling For Minimum T
double OversteerReqt = (LateralAccelerationComponent).Length() / This_Missile.MissileAccel;
if (OversteerReqt > 0.98)
{
LateralAccelerationComponent = This_Missile.MissileAccel * Vector3D.Normalize(LateralAccelerationComponent + (OversteerReqt * Vector3D.Normalize(-MissileVelocity)) * 40);
}
//Calculates And Applies Thrust In Correct Direction (Performs own inequality check)
double ThrustPower = RdavUtils.Vector_Projection_Scalar(MissileForwards, Vector3D.Normalize(LateralAccelerationComponent)); //TESTTESTTEST
ThrustPower = This_Missile.IsLargeGrid ? MathHelper.Clamp(ThrustPower, 0.9, 1) : ThrustPower;
//MathHelper.Clamp(ThrustPower, 0.6, 1); //for improved thrust performance
foreach (IMyThrust thruster in This_Missile.THRUSTERS)
{
if (thruster.ThrustOverride != (thruster.MaxThrust * ThrustPower)) //12 increment inequality to help conserve on performance
{
thruster.ThrustOverride = (float)(thruster.MaxThrust * ThrustPower);
}
}
//Calculates Remaining Force Component And Adds Along LOS
double RejectedAccel = Math.Sqrt(This_Missile.MissileAccel * This_Missile.MissileAccel - LateralAccelerationComponent.LengthSquared()); //Accel has to be determined whichever way you slice it
if (double.IsNaN(RejectedAccel))
{
RejectedAccel = 0;
}
LateralAccelerationComponent = LateralAccelerationComponent + LOS_New * RejectedAccel;
//-----------------------------------------------
//Guides To Target Using Gyros
am = Vector3D.Normalize(LateralAccelerationComponent + GravityComp);
double Yaw; double Pitch;
GyroTurn6(am, 18, 0.3, This_Missile.THRUSTERS[0], This_Missile.GYRO as IMyGyro, This_Missile.PREV_Yaw, This_Missile.PREV_Pitch, out Pitch, out Yaw);
//Updates For Next Tick Round
This_Missile.TARGET_PREV_POS = TargetPosition;
This_Missile.MIS_PREV_POS = MissilePosition;
This_Missile.PREV_Yaw = Yaw;
This_Missile.PREV_Pitch = Pitch;
//Detonates warheads in close proximity
if ((TargetPosition - MissilePosition).LengthSquared() < 20 * 20 && This_Missile.WARHEADS.Count > 0) //Arms
{
foreach (var item in This_Missile.WARHEADS)
{
(item as IMyWarhead).IsArmed = true;
}
}
if ((TargetPosition - MissilePosition).LengthSquared() < This_Missile.FuseDistance * This_Missile.FuseDistance && This_Missile.WARHEADS.Count > 0) //A mighty earth shattering kaboom
{
(This_Missile.WARHEADS[0] as IMyWarhead).Detonate();
}
}
void Guidance(Missile missile)
{
//Finds Current Target
Vector3D ENEMY_POS = EnemyScan(missile);
//---------------------------------------------------------------------------------------------------------------------------------
//Sorts CurrentVelocities
Vector3D MissilePosition = missile.m_gyroBlock.CubeGrid.WorldVolume.Center;
Vector3D MissilePositionPrev = missile.MIS_PREV_POS;
Vector3D MissileVelocity = (MissilePosition - MissilePositionPrev) / m_program.m_globalTimestep;
Vector3D TargetPosition = ENEMY_POS;
Vector3D TargetPositionPrev = missile.TARGET_PREV_POS;
Vector3D TargetVelocity = (TargetPosition - missile.TARGET_PREV_POS) / m_program.m_globalTimestep;
//Uses RdavNav Navigation APN Guidance System
//-----------------------------------------------
//Setup LOS rates and PN system
Vector3D LOS_Old = Vector3D.Normalize(TargetPositionPrev - MissilePositionPrev);
Vector3D LOS_New = Vector3D.Normalize(TargetPosition - MissilePosition);
Vector3D Rel_Vel = Vector3D.Normalize(TargetVelocity - MissileVelocity);
//And Assigners
Vector3D am = new Vector3D(1, 0, 0); double LOS_Rate; Vector3D LOS_Delta;
Vector3D MissileForwards = missile.m_thrusterBlockList[0].WorldMatrix.Backward;
//Vector/Rotation Rates
if (LOS_Old.Length() == 0)
{
LOS_Delta = new Vector3D(0, 0, 0); LOS_Rate = 0.0;
}
else
{
LOS_Delta = LOS_New - LOS_Old; LOS_Rate = LOS_Delta.Length() / m_program.m_globalTimestep;
}
//-----------------------------------------------
//Closing Velocity
double Vclosing = (TargetVelocity - MissileVelocity).Length();
//If Under Gravity Use Gravitational Accel
Vector3D GravityComp = -m_program.m_RC.GetNaturalGravity();
//Calculate the final lateral acceleration
Vector3D LateralDirection = Vector3D.Normalize(Vector3D.Cross(Vector3D.Cross(Rel_Vel, LOS_New), Rel_Vel));
Vector3D LateralAccelerationComponent = LateralDirection * m_program.m_PNGain * LOS_Rate * Vclosing + LOS_Delta * 9.8 * (0.5 * m_program.m_PNGain); //Eases Onto Target Collision LOS_Delta * 9.8 * (0.5 * Gain)
//If Impossible Solution (ie maxes turn rate) Use Drift Cancelling For Minimum T
double OversteerReqt = (LateralAccelerationComponent).Length() / missile.MissileAccel;
if (OversteerReqt > 0.98)
{
LateralAccelerationComponent = missile.MissileAccel * Vector3D.Normalize(LateralAccelerationComponent + (OversteerReqt * Vector3D.Normalize(-MissileVelocity)) * 40);
}
//Calculates And Applies Thrust In Correct Direction (Performs own inequality check)
double ThrustPower = RdavUtils.Vector_Projection_Scalar(MissileForwards, Vector3D.Normalize(LateralAccelerationComponent)); //TESTTESTTEST
ThrustPower = missile.IsLargeGrid ? MathHelper.Clamp(ThrustPower, 0.9, 1) : ThrustPower;
ThrustPower = MathHelper.Clamp(ThrustPower, 0.4, 1); //for improved thrust performance on the get-go
foreach (IMyThrust thruster in missile.m_thrusterBlockList)
{
if (thruster.ThrustOverride != (thruster.MaxThrust * ThrustPower)) //12 increment inequality to help conserve on performance
{
thruster.ThrustOverride = (float)(thruster.MaxThrust * ThrustPower);
}
}
//Calculates Remaining Force Component And Adds Along LOS
double RejectedAccel = Math.Sqrt(missile.MissileAccel * missile.MissileAccel - LateralAccelerationComponent.LengthSquared()); //Accel has to be determined whichever way you slice it
if (double.IsNaN(RejectedAccel))
{
RejectedAccel = 0;
}
LateralAccelerationComponent = LateralAccelerationComponent + LOS_New * RejectedAccel;
//-----------------------------------------------
//Guides To Target Using Gyros
am = Vector3D.Normalize(LateralAccelerationComponent + GravityComp);
double Yaw; double Pitch;
GyroTurn6(am, 18, 0.3, missile.m_thrusterBlockList[0], missile.m_gyroBlock as IMyGyro, missile.PREV_Yaw, missile.PREV_Pitch, out Pitch, out Yaw);
//Updates For Next Tick Round
missile.TARGET_PREV_POS = TargetPosition;
missile.MIS_PREV_POS = MissilePosition;
missile.PREV_Yaw = Yaw;
missile.PREV_Pitch = Pitch;
//Detonates warheads in close proximity
if ((TargetPosition - MissilePosition).LengthSquared() < 20 * 20 && missile.m_warheadBlockList.Count > 0) //Arms
{
foreach (var item in missile.m_warheadBlockList)
{
(item as IMyWarhead).IsArmed = true;
}
}
if ((TargetPosition - MissilePosition).LengthSquared() < missile.FuseDistance * missile.FuseDistance && missile.m_warheadBlockList.Count > 0) //A mighty earth shattering kaboom
{
(missile.m_warheadBlockList[0] as IMyWarhead).Detonate();
}
}
