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); }