private Vector3d CalculateImpactPositionWithAction()
{
var time_start = DateTime.Now;
double mass = State.Vessel.Mass;
double dry_mass = State.Vessel.DryMass;
Vector3d vel = State.Vessel.Velocity;
Vector3d pos = State.Vessel.Position;
double altitude = pos.Length() - CommonData.Body.Radius;
if (altitude <= TarAltitude)
{
return(pos);
}
double atm_depth = CommonData.Body.AtmosphereDepth;
bool enter_atm = false;
double next_burn_time = -1d;
double t = 0d;
double sim_lift_t = 0d;
double sim_lift_sum = 0d;
double sim_lift_min = double.MaxValue;
double sim_lift_max = double.MinValue;
double sim_thrust_sum = 0d;
double drag_sum = 0d;
double vel2_sum = 0d;
while (t < 1000d)
{
double atm = cache_.AtmAt(altitude);
double available_thrust = cache_.AvailableThrustAt(altitude);
Vector3d g = -pos * (CommonData.Body.GravitationalParameter / Math.Pow(CommonData.Body.Radius + altitude, 3));
double vel_mag = vel.Length();
var sim_data = new SimulationData
{
pos = pos,
vel = vel,
altitude = altitude,
pressure = atm,
g = g.Length(),
mass = mass,
tar_altitude = TarAltitude,
fuel_rate_at_full_throttle = State.Vessel.MaxFuelRate,
available_thrust = available_thrust,
t = t
};
SimulationResult sim_res = mass > dry_mass?Planner(sim_data) : new SimulationResult();
var thrust_acc = sim_res.Thrust / mass;
double left_height = altitude - TarAltitude;
double dt = Math.Min(1d, Math.Max(0.1d, left_height / Math.Max(1d, vel_mag) / 2d)) * t_ratio_;
if (sim_res.Throttle > 1e-3d && next_burn_time < 0d)
{
next_burn_time = t;
NextBurnTime = t;
}
// Lift estimation
if (t < LiftEstimationTime)
{
double sim_lift_mag = cache_.Lift(altitude, vel_mag);
sim_lift_t = t + dt;
sim_lift_sum += sim_lift_mag * dt;
sim_lift_min = Math.Min(sim_lift_min, sim_lift_mag);
sim_lift_max = Math.Max(sim_lift_max, sim_lift_mag);
sim_thrust_sum += sim_res.ThrustMag * dt;
}
double sim_drag_mag = cache_.Drift(altitude, vel_mag);
drag_sum += sim_drag_mag * dt;
vel2_sum += vel_mag * vel_mag * cache_.DensityAt(altitude) * dt;
Vector3d sim_drag_acc = -sim_drag_mag / mass * vel.Norm();
Vector3d acc = thrust_acc + g + sim_drag_acc;
//if (ut_before_calc_ - last_print_ut_ >= 1d)
// sw_.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t{6}\t{7}\t{8}",
// ut_before_calc_,
// vel,
// pos,
// sim_res.Throttle,
// mass,
// sim_data.available_thrust,
// g.Length(),
// sim_drag_mag,
// t);
t += dt;
vel += acc * dt;
Vector3d pos_last = pos;
double altitude_last = altitude;
pos += vel * dt;
mass -= State.Vessel.MaxFuelRate * sim_res.Throttle * dt;
altitude = pos.Length() - CommonData.Body.Radius;
if (CommonData.Body.HasAtmosphere)
{
if (!enter_atm && altitude <= atm_depth && altitude_last >= atm_depth)
{
enter_atm = true;
EnterAtmosphereDirection = -vel.Norm();
}
}
else
{
if (!enter_atm && next_burn_time < 0.5d)
{
enter_atm = true;
EnterAtmosphereDirection = -vel.Norm();
}
}
if (altitude <= TarAltitude)
{
double ratio = MathLib.InverseLerp(altitude_last, altitude, TarAltitude);
pos = MathLib.Lerp(pos_last, pos, ratio);
break;
}
}
last_print_ut_ = (ulong)ut_before_calc_;
ImpactTime = t;
LiftEstimationForceAve = sim_lift_t > 1e-3d ? sim_lift_sum / sim_lift_t : 0d;
LiftEstimationForceMin = sim_lift_min;
LiftEstimationThrustAve = sim_thrust_sum / sim_lift_t;
drag_ratio_ = drag_sum == 0d ? 0d : drag_sum / vel2_sum;
var time_used = (DateTime.Now - time_start).TotalMilliseconds;
t_ave_ = Math.Max(time_used, 1d);
t_ratio_ *= (t_ave_ / 100d - 1d) * 0.2d + 1d;
t_ratio_ = Math.Clamp(t_ratio_, 0.01d, 100d);
ResultStable = t_ratio_ < 1d;
return(pos);
}
