private Vector3 CruiseGuidance()
{
Vector3 cruiseTarget = Vector3.zero;
float distance = Vector3.Distance(TargetPosition, vessel.CoM);
if (distance < 4500)
{
cruiseTarget = MissileGuidance.GetTerminalManeuveringTarget(TargetPosition, vessel, CruiseAltitude);
debugString += "\nTerminal Maneuvers";
}
else
{
float agmThreshDist = 2500;
if (distance < agmThreshDist)
{
if (!MissileGuidance.GetBallisticGuidanceTarget(TargetPosition, vessel, true, out cruiseTarget))
{
cruiseTarget = MissileGuidance.GetAirToGroundTarget(TargetPosition, vessel, 1.85f);
}
debugString += "\nDescending On Target";
}
else
{
cruiseTarget = MissileGuidance.GetCruiseTarget(TargetPosition, vessel, CruiseAltitude);
debugString += "\nCruising";
}
}
debugString += "\nRadarAlt: " + MissileGuidance.GetRadarAltitude(vessel);
return(cruiseTarget);
}
public bool GetLaunchAuthorization(Vessel targetV, MissileFire mf)
{
bool launchAuthorized = false;
Vector3 target = targetV.transform.position;
MissileLauncher missile = mf.currentMissile;
if (missile != null)
{
if (!targetV.Landed)
{
target = MissileGuidance.GetAirToAirFireSolution(missile, targetV);
}
float boresightFactor = targetV.Landed ? 0.75f : 0.35f;
float maxOffBoresight = missile.maxOffBoresight;
if (missile.targetingMode == MissileLauncher.TargetingModes.GPS)
{
maxOffBoresight = 45;
}
float fTime = 2f;
Vector3 futurePos = target + (targetV.srf_velocity * fTime);
Vector3 myFuturePos = vesselTransform.position + (vessel.srf_velocity * fTime);
bool fDot = Vector3.Dot(vesselTransform.up, futurePos - myFuturePos) > 0; //check target won't likely be behind me soon
if (fDot && Vector3.Angle(missile.transform.forward, target - missile.transform.position) < maxOffBoresight * boresightFactor)
{
launchAuthorized = true;
}
}
return(launchAuthorized);
}
private Vector3 AGMGuidance()
{
if (TargetingMode != TargetingModes.Gps)
{
if (TargetAcquired)
{
//lose lock if seeker reaches gimbal limit
float targetViewAngle = Vector3.Angle(vessel.transform.forward, TargetPosition - vessel.CoM);
if (targetViewAngle > maxOffBoresight)
{
Debug.Log("[BDArmory]: AGM Missile guidance failed - target out of view");
guidanceActive = false;
}
}
else
{
if (TargetingMode == TargetingModes.Laser)
{
//keep going straight until found laser point
TargetPosition = laserStartPosition + (20000 * startDirection);
}
}
}
Vector3 agmTarget = MissileGuidance.GetAirToGroundTarget(TargetPosition, vessel, 1.85f);
return(agmTarget);
}
private Vector3 AAMGuidance()
{
Vector3 aamTarget;
if (TargetAcquired)
{
float timeToImpact;
aamTarget = MissileGuidance.GetAirToAirTargetModular(TargetPosition, TargetVelocity, previousTargetVelocity, TargetAcceleration, vessel, previousMissileVelocity, out timeToImpact);
previousTargetVelocity = TargetVelocity;
previousMissileVelocity = vessel.srf_velocity;
TimeToImpact = timeToImpact;
if (Vector3.Angle(aamTarget - vessel.CoM, vessel.transform.forward) > maxOffBoresight * 0.75f)
{
Debug.LogFormat("[BDArmory]: Missile with Name={0} has exceeded the max off boresight, checking missed target ", vessel.vesselName);
aamTarget = TargetPosition;
}
DrawDebugLine(vessel.CoM, aamTarget);
}
else
{
aamTarget = vessel.CoM + (20 * vessel.srfSpeed * vessel.srf_velocity.normalized);
}
return(aamTarget);
}
public void GuidanceSteer(FlightCtrlState s)
{
if (guidanceActive && targetVessel != null && vessel != null && vesselTransform != null && velocityTransform != null)
{
velocityTransform.rotation = Quaternion.LookRotation(vessel.srf_velocity, -vesselTransform.forward);
Vector3 targetPosition = targetVessel.CoM;
Vector3 localAngVel = vessel.angularVelocity;
if (guidanceMode == 1)
{
targetPosition = MissileGuidance.GetAirToAirTarget(targetPosition, vessel.srf_velocity, vessel.acceleration, vessel, out timeToImpact);
}
else if (guidanceMode == 2)
{
targetPosition = MissileGuidance.GetAirToGroundTarget(targetPosition, vessel, 1.85f);
}
else
{
targetPosition = MissileGuidance.GetCruiseTarget(targetPosition, vessel, cruiseAltitude);
}
Vector3 targetDirection = velocityTransform.InverseTransformPoint(targetPosition).normalized;
targetDirection = Vector3.RotateTowards(Vector3.forward, targetDirection, 15 * Mathf.Deg2Rad, 0);
float steerYaw = (steerMult * targetDirection.x) - (steerDamping * -localAngVel.z);
float steerPitch = (steerMult * targetDirection.y) - (steerDamping * -localAngVel.x);
s.yaw = Mathf.Clamp(steerYaw, -maxSteer, maxSteer);
s.pitch = Mathf.Clamp(steerPitch, -maxSteer, maxSteer);
s.mainThrottle = 1;
}
}
void UpdateTarget()
{
slaved = false;
if (weaponManager && wm.slavingTurrets && wm.currentMissile)
{
slaved = true;
slavedTargetPosition = MissileGuidance.GetAirToAirFireSolution(wm.currentMissile, wm.slavedPosition, wm.slavedVelocity);
}
}
void RegainEnergy(FlightCtrlState s)
{
debugString += "\nRegaining energy";
steerMode = SteerModes.Aiming;
Vector3 planarDirection = Vector3.ProjectOnPlane(vessel.srf_velocity, upDirection);
float angle = (Mathf.Clamp(MissileGuidance.GetRadarAltitude(vessel) - minAltitude, 0, 1500) / 1500) * 90;
angle = Mathf.Clamp(angle, 0, 55) * Mathf.Deg2Rad;
Vector3 targetDirection = Vector3.RotateTowards(planarDirection, -upDirection, angle, 0).normalized;
AdjustThrottle(maxSpeed, false);
FlyToPosition(s, vesselTransform.position + (targetDirection * 100));
}
private Vector3 BallisticGuidance()
{
Vector3 agmTarget;
bool validSolution = MissileGuidance.GetBallisticGuidanceTarget(TargetPosition, vessel, false, out agmTarget);
if (!validSolution || Vector3.Angle(TargetPosition - this.vessel.CoM, agmTarget - this.vessel.CoM) > Mathf.Clamp(maxOffBoresight, 0, 65))
{
Vector3 dToTarget = TargetPosition - this.vessel.CoM;
Vector3 direction = Quaternion.AngleAxis(Mathf.Clamp(maxOffBoresight * 0.9f, 0, 45f), Vector3.Cross(dToTarget, VectorUtils.GetUpDirection(this.vessel.transform.position))) * dToTarget;
agmTarget = this.vessel.CoM + direction;
}
return(agmTarget);
}
Vector3 FlightPosition(Vector3 targetPosition, float minAlt)
{
Vector3 forwardDirection = vesselTransform.up;
Vector3 targetDirection = (targetPosition - vesselTransform.position).normalized;
if (Vector3.Dot(targetDirection, forwardDirection) < 0)
{
targetPosition = vesselTransform.position + Vector3.RotateTowards(Vector3.ProjectOnPlane(forwardDirection, upDirection), Vector3.ProjectOnPlane(targetDirection, upDirection), 90 * Mathf.Deg2Rad, 0).normalized *200;
}
float pointRadarAlt = MissileGuidance.GetRaycastRadarAltitude(targetPosition);
if (pointRadarAlt < minAlt)
{
float adjustment = (minAlt - pointRadarAlt);
debugString += "\nTarget position is below minAlt. Adjusting by " + adjustment;
return(targetPosition + (adjustment * upDirection));
}
else
{
return(targetPosition);
}
}
void TakeOff(FlightCtrlState s)
{
threatLevel = 1;
debugString += "\nTaking off/Gaining altitude";
if (vessel.Landed && vessel.srfSpeed < takeOffSpeed)
{
defaultOrbitCoords = VectorUtils.WorldPositionToGeoCoords(vessel.transform.position, vessel.mainBody);
return;
}
steerMode = SteerModes.Aiming;
float radarAlt = MissileGuidance.GetRadarAltitude(vessel);
Vector3 forwardPoint = vessel.transform.position + Vector3.ProjectOnPlane(vesselTransform.up * 100, upDirection);
float terrainDiff = MissileGuidance.GetRaycastRadarAltitude(forwardPoint) - radarAlt;
terrainDiff = Mathf.Max(terrainDiff, 0);
float rise = Mathf.Clamp((float)vessel.srfSpeed * 0.3f, 5, 100);
if (radarAlt > 70)
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false);
}
else
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true);
}
FlyToPosition(s, forwardPoint + (upDirection * (rise + terrainDiff)));
if (radarAlt > minAltitude)
{
startedLanded = false;
}
}
bool DetectCollision(Vector3 direction)
{
if (MissileGuidance.GetRadarAltitude(vessel) < 20)
{
return(false);
}
direction = direction.normalized;
int layerMask = 557057;
Ray ray = new Ray(vesselTransform.position + (50 * vesselTransform.up), direction);
float distance = Mathf.Clamp((float)vessel.srfSpeed * 5, 250, 2500);
RaycastHit hit;
if (Physics.SphereCast(ray, 10, out hit, distance, layerMask))
{
Rigidbody otherRb = hit.collider.attachedRigidbody;
if (otherRb)
{
if (Vector3.Dot(otherRb.velocity, vessel.srf_velocity) < 0)
{
return(true);
}
else
{
return(false);
}
}
else
{
return(true);
}
}
else
{
return(false);
}
}
Vector3 GetTerrainSurfacePosition(Vector3 position)
{
return(position - (MissileGuidance.GetRaycastRadarAltitude(position) * upDirection));
}
void FlyToTargetVessel(FlightCtrlState s, Vessel v)
{
Vector3 target = v.CoM;
MissileLauncher missile = null;
Vector3 vectorToTarget = v.transform.position - vesselTransform.position;
float distanceToTarget = vectorToTarget.magnitude;
if (weaponManager)
{
missile = weaponManager.currentMissile;
if (missile != null)
{
if (missile.targetingMode == MissileLauncher.TargetingModes.Heat && !weaponManager.heatTarget.exists)
{
target += v.srf_velocity.normalized * 10;
}
else
{
target = MissileGuidance.GetAirToAirFireSolution(missile, v);
}
if (Vector3.Angle(target - vesselTransform.position, vesselTransform.forward) < 15)
{
steerMode = SteerModes.Aiming;
}
}
else
{
ModuleWeapon weapon = weaponManager.currentGun;
if (weapon != null)
{
//target -= 1.30f*weapon.GetLeadOffset();
Vector3 leadOffset = weapon.GetLeadOffset();
float targetAngVel = 1.65f * Vector3.Angle(v.transform.position - vessel.transform.position, v.transform.position + (vessel.srf_velocity) - vessel.transform.position);
debugString += "\ntargetAngVel: " + targetAngVel;
float magnifier = Mathf.Clamp(targetAngVel, 1.25f, 5);
target -= magnifier * leadOffset;
float angleToLead = Vector3.Angle(vesselTransform.up, target - vesselTransform.position);
if (distanceToTarget < 1600 && angleToLead < 20)
{
steerMode = SteerModes.Aiming; //steer to aim
}
}
}
}
float targetDot = Vector3.Dot(vesselTransform.up, v.transform.position - vessel.transform.position);
//manage speed when close to enemy
float finalMaxSpeed = ((distanceToTarget - 100) / 8) + (float)v.srfSpeed;
AdjustThrottle(finalMaxSpeed, true);
if ((targetDot < 0 || vessel.srfSpeed > finalMaxSpeed) &&
distanceToTarget < 800) //distance is less than 800m
{
debugString += ("\nEnemy on tail. Braking");
AdjustThrottle(minSpeed, true);
}
if (missile != null &&
targetDot > 0 &&
distanceToTarget < 300 &&
vessel.srfSpeed > 130)
{
extending = true;
lastTargetPosition = v.transform.position;
}
FlyToPosition(s, target);
}
void AutoPilot(FlightCtrlState s)
{
if (!vessel || !vessel.transform || vessel.packed || !vessel.mainBody)
{
return;
}
vesselTransform = vessel.ReferenceTransform;
//default brakes off full throttle
//s.mainThrottle = 1;
//vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false);
AdjustThrottle(maxSpeed, true);
useAB = true;
useBrakes = true;
vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
steerMode = SteerModes.NormalFlight;
GetGuardTarget();
if (vessel.Landed && standbyMode && weaponManager && BDATargetManager.TargetDatabase[BDATargetManager.BoolToTeam(weaponManager.team)].Count == 0)
{
//s.mainThrottle = 0;
//vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
AdjustThrottle(0, true);
return;
}
//upDirection = -FlightGlobals.getGeeForceAtPosition(transform.position).normalized;
upDirection = VectorUtils.GetUpDirection(vessel.transform.position);
debugString = string.Empty;
if (MissileGuidance.GetRadarAltitude(vessel) < minAltitude)
{
startedLanded = true;
}
if (startedLanded)
{
TakeOff(s);
turningTimer = 0;
}
else
{
if (FlyAvoidCollision(s))
{
turningTimer = 0;
}
else if (command != PilotCommands.Free)
{
UpdateCommand(s);
}
else
{
UpdateAI(s);
}
}
//brake and cut throttle if exceeding max speed
/*
* if(vessel.srfSpeed > maxSpeed)
* {
* vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
* s.mainThrottle = 0;
* }
*/
debugString += "\nthreatLevel: " + threatLevel;
}