public Vector3 GetDirection(MissileBase missile, Vector3 targetPosition, Vector3 targetVelocity)
{
//set up
if (_missile.TimeIndex < 1)
{
return(_missile.vessel.CoM + _missile.vessel.Velocity() * 10);
}
upDirection = VectorUtils.GetUpDirection(_missile.vessel.CoM);
planarDirectionToTarget =
Vector3.ProjectOnPlane(targetPosition - _missile.vessel.CoM, upDirection).normalized;
// Ascending
_missile.debugString.Append("State=" + GuidanceState);
_missile.debugString.Append(Environment.NewLine);
var missileAltitude = GetCurrentAltitude(_missile.vessel);
_missile.debugString.Append("Altitude=" + missileAltitude);
_missile.debugString.Append(Environment.NewLine);
_missile.debugString.Append("Apoapsis=" + _missile.vessel.orbit.ApA);
_missile.debugString.Append(Environment.NewLine);
_missile.debugString.Append("Future Altitude=" + _futureAltitude);
_missile.debugString.Append(Environment.NewLine);
_missile.debugString.Append("Pitch angle=" + _pitchAngle);
_missile.debugString.Append(Environment.NewLine);
_missile.debugString.Append("Pitch decision=" + PitchDecision);
_missile.debugString.Append(Environment.NewLine);
_missile.debugString.Append("lastVerticalSpeed=" + _lastVerticalSpeed);
_missile.debugString.Append(Environment.NewLine);
_missile.debugString.Append("verticalAcceleration=" + _verticalAcceleration);
_missile.debugString.Append(Environment.NewLine);
GetTelemetryData();
switch (GuidanceState)
{
case GuidanceState.Ascending:
UpdateThrottle();
if (MissileWillReachAltitude(missileAltitude))
{
_pitchAngle = 0;
GuidanceState = GuidanceState.Cruising;
break;
}
CheckIfTerminal(missileAltitude, targetPosition, upDirection);
return(_missile.vessel.CoM + (planarDirectionToTarget.normalized + upDirection.normalized) * 10f);
case GuidanceState.Cruising:
CheckIfTerminal(missileAltitude, targetPosition, upDirection);
//Altitude control
UpdatePitch(missileAltitude);
UpdateThrottle();
return(_missile.vessel.CoM + 10 * planarDirectionToTarget.normalized + _pitchAngle * upDirection);
case GuidanceState.Terminal:
_missile.debugString.Append($"Descending");
_missile.debugString.Append(Environment.NewLine);
_missile.Throttle = Mathf.Clamp((float)(_missile.vessel.atmDensity * 10f), 0.01f, 1f);
if (_missile is BDModularGuidance)
{
if (_missile.vessel.InVacuum())
{
return(_missile.vessel.CoM + _missile.vessel.Velocity() * 10);
}
}
return(MissileGuidance.GetAirToGroundTarget(targetPosition, targetVelocity, _missile.vessel, 1.85f));
}
return(_missile.vessel.CoM + _missile.vessel.Velocity() * 10);
}
private double GetCurrentAltitude(Vessel missileVessel)
{
var currentRadarAlt = MissileGuidance.GetRadarAltitude(missileVessel);
return(currentRadarAlt);
}
private double GetCurrentAltitudeAtPosition(Vector3 position)
{
var currentRadarAlt = MissileGuidance.GetRadarAltitudeAtPos(position);
return(currentRadarAlt);
}
public Vector3 GetDirection(MissileBase missile, Vector3 targetPosition)
{
//set up
if (_originalDistance == float.MinValue)
{
_startPoint = missile.vessel.CoM;
_originalDistance = Vector3.Distance(targetPosition, missile.vessel.CoM);
}
var surfaceDistanceVector = Vector3
.Project((missile.vessel.CoM - _startPoint), (targetPosition - _startPoint).normalized);
var pendingDistance = _originalDistance - surfaceDistanceVector.magnitude;
if (missile.TimeIndex < 1)
{
return(missile.vessel.CoM + missile.vessel.Velocity() * 10);
}
Vector3 agmTarget;
if (missile.vessel.verticalSpeed > 0 && pendingDistance > _originalDistance * 0.5)
{
missile.debugString.Append($"Ascending");
missile.debugString.Append(Environment.NewLine);
var freeFallTime = CalculateFreeFallTime(missile);
missile.debugString.Append($"freeFallTime: {freeFallTime}");
missile.debugString.Append(Environment.NewLine);
var futureDistanceVector = Vector3
.Project((missile.vessel.GetFuturePosition() - _startPoint), (targetPosition - _startPoint).normalized);
var futureHorizontalSpeed = CalculateFutureHorizontalSpeed(missile);
var horizontalTime = (_originalDistance - futureDistanceVector.magnitude) / futureHorizontalSpeed;
missile.debugString.Append($"horizontalTime: {horizontalTime}");
missile.debugString.Append(Environment.NewLine);
if (freeFallTime >= horizontalTime)
{
missile.debugString.Append($"Free fall achieved:");
missile.debugString.Append(Environment.NewLine);
missile.Throttle = Mathf.Clamp(missile.Throttle - 0.001f, 0.01f, 1f);
}
else
{
missile.debugString.Append($"Free fall not achieved:");
missile.debugString.Append(Environment.NewLine);
missile.Throttle = Mathf.Clamp(missile.Throttle + 0.001f, 0.01f, 1f);
}
Vector3 dToTarget = targetPosition - missile.vessel.CoM;
Vector3 direction = Quaternion.AngleAxis(Mathf.Clamp(missile.maxOffBoresight * 0.9f, 0, missile.BallisticAngle), Vector3.Cross(dToTarget, VectorUtils.GetUpDirection(missile.vessel.CoM))) * dToTarget;
agmTarget = missile.vessel.CoM + direction;
missile.debugString.Append($"Throttle: {missile.Throttle}");
missile.debugString.Append(Environment.NewLine);
}
else
{
missile.debugString.Append($"Descending");
missile.debugString.Append(Environment.NewLine);
agmTarget = MissileGuidance.GetAirToGroundTarget(targetPosition, missile.vessel, 1.85f);
missile.Throttle = Mathf.Clamp((float)(missile.vessel.atmDensity * 10f), 0.01f, 1f);
}
if (missile is BDModularGuidance)
{
if (missile.vessel.InVacuum())
{
missile.vessel.Autopilot.SetMode(VesselAutopilot.AutopilotMode.Prograde);
agmTarget = missile.vessel.CoM + missile.vessel.Velocity() * 100;
}
else
{
missile.vessel.Autopilot.SetMode(VesselAutopilot.AutopilotMode.StabilityAssist);
}
}
return(agmTarget);
}
