public DState RungeKutta(ref SimulationState s, double dt)
{
DState ds1 = s.derivate();
DState ds2 = s.increment(ds1, dt / 2).derivate();
DState ds3 = s.increment(ds2, dt / 2).derivate();
DState ds4 = s.increment(ds3, dt).derivate();
s = s.increment(ds1, dt / 6).increment(ds2, dt / 3).increment(ds3, dt / 3).increment(ds4, dt / 6);
return(ds1);
}
public DState derivate()
{
DState res = new DState();
res.vx = vx;
res.vy = vy;
double r = this.r;
float altitude = (float)(r - planet.Radius);
double theta = Math.Atan2(u_x, u_y);
double thrustDirection = theta + ascentPath.FlightPathAngle(altitude);
// gravity
double grav_acc = -planet.gravParameter / (r * r);
// drag
var dragForce = StockAeroUtil.SimAeroForce(availableNodes.Keys.ToList(), planet, new UnityEngine.Vector3(0, (float)v_surf, 0), altitude);
double drag_acc = dragForce.magnitude / m;
//throttle
double desiredThrust = maxAcceleration * m;
if (maxThrust != minThrust)
{
throttle = ((float)desiredThrust - minThrust) / (maxThrust - minThrust);
}
else
{
throttle = 1;
}
throttle = Math.Max(0, Math.Min(1, throttle));
// Effective thrust
double F = activeEngines.Sum(e => e.thrust(throttle, pressure, machNumber, atmDensity));
// Propellant mass variation
res.dm = -activeEngines.Sum(e => e.evaluateFuelFlow(atmDensity, machNumber, throttle));
res.ax_nograv = F / m * Math.Sin(thrustDirection);
res.ay_nograv = F / m * Math.Cos(thrustDirection);
if (v_surf != 0)
{
res.ax_nograv -= drag_acc * v_surf_x / v_surf;
res.ay_nograv -= drag_acc * v_surf_y / v_surf;
}
res.ax = res.ax_nograv + grav_acc * u_x;
res.ay = res.ay_nograv + grav_acc * u_y;
return(res);
}
public SimulationState increment(DState delta, double dt)
{
SimulationState res = (SimulationState) MemberwiseClone();
res.x += dt * delta.vx;
res.y += dt * delta.vy;
res.vx += dt * delta.ax;
res.vy += dt * delta.ay;
res.m += dt * delta.dm;
if (r2 <= res.planet.Radius * res.planet.Radius)
{
double r = res.r;
res.x *= res.planet.Radius / r;
res.y *= res.planet.Radius / r;
res.vx = res.y * (planet.rotates ? (2 * Math.PI / planet.rotationPeriod) : 0);
res.vy = - res.x * (planet.rotates ? (2 * Math.PI / planet.rotationPeriod) : 0);
}
return res;
}
public SimulationState increment(DState delta, double dt)
{
SimulationState res = (SimulationState)MemberwiseClone();
res.x += dt * delta.vx;
res.y += dt * delta.vy;
res.vx += dt * delta.ax;
res.vy += dt * delta.ay;
res.m += dt * delta.dm;
if (r2 <= res.planet.Radius * res.planet.Radius)
{
double r = res.r;
res.x *= res.planet.Radius / r;
res.y *= res.planet.Radius / r;
res.vx = res.y * (planet.rotates ? (2 * Math.PI / planet.rotationPeriod) : 0);
res.vy = -res.x * (planet.rotates ? (2 * Math.PI / planet.rotationPeriod) : 0);
}
return(res);
}
public void computeStages()
{
#if DEBUG
DateTime startTime = DateTime.Now;
#endif
double elapsedTime = 0;
while (state.availableNodes.Count() > 0)
{
if (advancedSimulation)
{
state.m = state.availableNodes.Sum(p => p.Value.mass);
// Compute flow for active engines
foreach (EngineWrapper e in state.activeEngines)
{
e.evaluateFuelFlow(state.atmDensity, state.machNumber, state.throttle, false);
}
}
else
{
// Compute flow for active engines, in vacuum
foreach (EngineWrapper e in state.activeEngines)
{
e.evaluateFuelFlow(1, 1, 1, false);
}
}
double step = Math.Max(state.availableNodes.Min(node => node.Value.getNextEvent()), 1E-100);
// Quit if there is no other event
if (step == Double.MaxValue && state.throttle > 0)
{
break;
}
if (advancedSimulation)
{
if (step > simulationStep)
{
step = Math.Max(simulationStep, (elapsedTime + step - stages.Last().activationTime) / 100);
}
if (state.throttle == 0)
{
step = simulationStep;
}
float throttle = state.throttle;
var savedState = state;
DState dState = RungeKutta(ref state, step);
while (Math.Abs(state.throttle - throttle) > 0.05 && step > 1e-3)
{
state = savedState;
step /= 2;
dState = RungeKutta(ref state, step);
}
Sample sample;
sample.time = elapsedTime + step;
sample.velocity = state.v_surf;
sample.altitude = state.r - state.planet.Radius;
sample.mass = state.m;
sample.acceleration = Math.Sqrt(dState.ax_nograv * dState.ax_nograv + dState.ay_nograv * dState.ay_nograv);
sample.throttle = state.throttle;
if (samples.Count == 0 || samples.Last().time + simulationStep <= sample.time)
{
samples.Add(sample);
}
}
elapsedTime += step;
// Burn the fuel !
bool eventHappens = false;
foreach (Node node in state.availableNodes.Values)
{
eventHappens |= node.applyFuelConsumption(step);
}
if (!eventHappens)
{
continue;
}
// Add all decouplers in a new stage
StageDescription newStage = new StageDescription(elapsedTime);
foreach (Node node in state.availableNodes.Values)
{
ModuleDecouple decoupler = node.part.Modules.OfType <ModuleDecouple>().FirstOrDefault();
ModuleAnchoredDecoupler aDecoupler = node.part.Modules.OfType <ModuleAnchoredDecoupler>().FirstOrDefault();
if ((decoupler != null || aDecoupler != null) && !node.hasFuelInChildren(state.availableNodes))
{
newStage.stageParts.Add(node.part);
}
}
if (newStage.stageParts.Count > 0)
{
stages.Add(newStage);
List <Part> activableChildren = new List <Part>();
// Remove all decoupled elements, fire sepratrons and parachutes
foreach (Part part in newStage.stageParts)
{
if (state.availableNodes.ContainsKey(part))
{
activableChildren.AddRange(state.availableNodes[part].getRelevantChildrenOnDecouple(state.availableNodes));
dropPartAndChildren(part);
}
}
newStage.stageParts.AddRange(activableChildren);
}
// Update available engines and fuel flow
List <Part> activeEngines = state.updateEngines();
if (newStage.stageParts.Count > 0)
{
newStage.stageParts.AddRange(activeEngines);
}
}
// Put fairings in a separate stage before decoupling
List <StageDescription> newStages = new List <StageDescription>();
for (int i = 0; i < stages.Count; i++)
{
var fairings = stages[i].stageParts.Where(p => p.Modules.OfType <ModuleProceduralFairing>().Count() != 0);
var notfairings = stages[i].stageParts.Where(p => p.Modules.OfType <ModuleProceduralFairing>().Count() == 0);
if (fairings.Count() != 0)
{
StageDescription fairingStage = new StageDescription(stages[i].activationTime);
fairingStage.stageParts.AddRange(fairings);
newStages.Add(fairingStage);
}
StageDescription notfairingStage = new StageDescription(stages[i].activationTime);
notfairingStage.stageParts.AddRange(notfairings);
newStages.Add(notfairingStage);
}
stages = newStages;
// Put all remaining items (parachutes?) in a separate 0 stage
foreach (var stage in stages)
{
foreach (var part in stage.stageParts)
{
state.availableNodes.Remove(part);
}
}
var stage0 = new StageDescription(elapsedTime);
stage0.stageParts = state.availableNodes.Keys.Where(p => p.hasStagingIcon).ToList();
if (stage0.stageParts.Count != 0)
{
stages.Add(stage0);
}
stages.Reverse();
// This is ugly, but on KSP 1.1 I have not found anything better
// We call this private method for each part from the root, twice and it works...
System.Reflection.MethodInfo SortIcons = null;
// In KSP 1.10, an extra parameter was added to SortIcons
if (Versioning.version_major == 1 && Versioning.version_minor >= 10)
{
SortIcons = typeof(KSP.UI.Screens.StageManager).GetMethod("SortIcons",
System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Instance, null,
new Type[] { typeof(bool), typeof(Part), typeof(bool), typeof(bool) }, null);
}
else
{
SortIcons = typeof(KSP.UI.Screens.StageManager).GetMethod("SortIcons",
System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Instance, null,
new Type[] { typeof(bool), typeof(Part), typeof(bool) }, null);
}
var root = stages[0].stageParts[0];
while (root.parent != null)
{
root = root.parent;
}
setStages(root, SortIcons);
// setStages(root, SortIcons);
#if DEBUG
var compTime = DateTime.Now - startTime;
Debug.Log("Staging computed in " + compTime.TotalMilliseconds + "ms");
if (samples.Count() > 0)
{
string result = "time;altitude;velocity;acceleration;mass;throttle\n";
foreach (var sample in samples)
{
result += sample.time + ";" + sample.altitude + ";" + sample.velocity + ";" + sample.acceleration + ";" + sample.mass + ";" + sample.throttle + "\n";
}
Debug.Log(result);
}
#endif
}
public DState derivate()
{
DState res = new DState();
res.vx = vx;
res.vy = vy;
double r = this.r;
float altitude = (float) (r - planet.Radius);
double theta = Math.Atan2(u_x, u_y);
double thrustDirection = theta + ascentPath.FlightPathAngle(altitude);
// gravity
double grav_acc = -planet.gravParameter / (r * r);
// drag
var dragForce = StockAeroUtil.SimAeroForce(availableNodes.Keys.ToList(), planet, new UnityEngine.Vector3(0,(float)v_surf, 0), altitude);
double drag_acc = dragForce.magnitude / m;
//throttle
double desiredThrust = maxAcceleration * m;
if (maxThrust != minThrust)
throttle = ((float)desiredThrust - minThrust) / (maxThrust - minThrust);
else
throttle = 1;
throttle = Math.Max(0, Math.Min(1, throttle));
// Effective thrust
double F = activeEngines.Sum(e => e.thrust(throttle, pressure, machNumber, atmDensity));
// Propellant mass variation
res.dm = - activeEngines.Sum(e => e.evaluateFuelFlow(atmDensity, machNumber, throttle));
res.ax_nograv = F / m * Math.Sin(thrustDirection);
res.ay_nograv = F / m * Math.Cos(thrustDirection);
if (v_surf != 0)
{
res.ax_nograv -= drag_acc * v_surf_x/v_surf;
res.ay_nograv -= drag_acc * v_surf_y/v_surf;
}
res.ax = res.ax_nograv + grav_acc * u_x;
res.ay = res.ay_nograv + grav_acc * u_y;
return res;
}
public void computeStages()
{
#if DEBUG
DateTime startTime = DateTime.Now;
#endif
double elapsedTime = 0;
while (state.availableNodes.Count() > 0)
{
if (advancedSimulation)
{
state.m = state.availableNodes.Sum(p => p.Value.mass);
// Compute flow for active engines
foreach (EngineWrapper e in state.activeEngines)
{
e.evaluateFuelFlow(state.atmDensity, state.machNumber, state.throttle, false);
}
}
else
{
// Compute flow for active engines, in vacuum
foreach (EngineWrapper e in state.activeEngines)
{
e.evaluateFuelFlow(1, 1, 1, false);
}
}
double step = Math.Max(state.availableNodes.Min(node => node.Value.getNextEvent()), 1E-100);
// Quit if there is no other event
if (step == Double.MaxValue && state.throttle > 0)
{
break;
}
if (advancedSimulation)
{
if (step > simulationStep)
{
step = Math.Max(simulationStep, (elapsedTime + step - stages.Last().activationTime) / 100);
}
if (state.throttle == 0)
{
step = simulationStep;
}
float throttle = state.throttle;
var savedState = state;
DState dState = RungeKutta(ref state, step);
while (Math.Abs(state.throttle - throttle) > 0.05 && step > 1e-3)
{
state = savedState;
step /= 2;
dState = RungeKutta(ref state, step);
}
Sample sample;
sample.time = elapsedTime + step;
sample.velocity = state.v_surf;
sample.altitude = state.r - state.planet.Radius;
sample.mass = state.m;
sample.acceleration = Math.Sqrt(dState.ax_nograv * dState.ax_nograv + dState.ay_nograv * dState.ay_nograv);
sample.throttle = state.throttle;
if (samples.Count == 0 || samples.Last().time + simulationStep <= sample.time)
{
samples.Add(sample);
}
}
elapsedTime += step;
// Burn the fuel !
bool eventHappens = false;
foreach (Node node in state.availableNodes.Values)
{
eventHappens |= node.applyFuelConsumption(step);
}
if (!eventHappens)
{
continue;
}
// Add all decouplers in a new stage
StageDescription dpStage = new StageDescription(elapsedTime);
foreach (Node node in state.availableNodes.Values)
{
ModuleDecouple decoupler = node.part.Modules.OfType <ModuleDecouple>().FirstOrDefault();
ModuleAnchoredDecoupler aDecoupler = node.part.Modules.OfType <ModuleAnchoredDecoupler>().FirstOrDefault();
if ((decoupler != null || aDecoupler != null) && !node.hasFuelInChildren(state.availableNodes))
{
dpStage.stageParts.Add(node.part);
}
}
if (dpStage.stageParts.Count > 0)
{
stages.Add(dpStage);
List <Part> activableChildren = new List <Part>();
// Remove all decoupled elements, fire sepratrons and parachutes
foreach (Part part in dpStage.stageParts)
{
if (state.availableNodes.ContainsKey(part))
{
activableChildren.AddRange(state.availableNodes[part].getRelevantChildrenOnDecouple(state.availableNodes));
dropPartAndChildren(part);
}
}
dpStage.stageParts.AddRange(activableChildren);
}
// Update available engines and fuel flow
List <Part> activeEngines = state.updateEngines();
StageDescription newStage = new StageDescription(elapsedTime);
newStage.stageParts.AddRange(activeEngines);
stages.Add(newStage);
}
// Put all remaining items (parachutes?) in a separate 0 stage
foreach (var stage in stages)
{
foreach (var part in stage.stageParts)
{
state.availableNodes.Remove(part);
}
}
// Put fairings in a separate stage before decoupling
List <StageDescription> newStages = new List <StageDescription>();
for (int i = 0; i < stages.Count; i++)
{
var fairings = stages[i].stageParts.Where(p => p.Modules.OfType <ModuleProceduralFairing>().Count() != 0);
var notfairings = stages[i].stageParts.Where(p => p.Modules.OfType <ModuleProceduralFairing>().Count() == 0);
if (fairings.Count() != 0)
{
StageDescription fairingStage = new StageDescription(stages[i].activationTime);
fairingStage.stageParts.AddRange(fairings);
newStages.Add(fairingStage);
}
StageDescription notfairingStage = new StageDescription(stages[i].activationTime);
notfairingStage.stageParts.AddRange(notfairings);
newStages.Add(notfairingStage);
}
stages = newStages;
int initialStage = 0;
foreach (Part part in state.availableNodes.Keys)
{
if (part.hasStagingIcon)
{
part.inverseStage = 0;
initialStage = 1;
}
}
// doesn't seem to work in 1.1
// Set stage number correctly
//StageManager.SetStageCount (stages.Count);
for (int stage = stages.Count - 1; stage >= 0; stage--)
{
var currentStage = stages[stage];
foreach (Part part in currentStage.stageParts)
{
part.inverseStage = stages.Count - 1 - stage + initialStage;
}
}
// doesn't seem to work in 1.1
//StageManager.Instance.SortIcons (true);
#if DEBUG
var compTime = DateTime.Now - startTime;
Debug.Log("Staging computed in " + compTime.TotalMilliseconds + "ms");
if (samples.Count() > 0)
{
string result = "time;altitude;velocity;acceleration;mass;throttle\n";
foreach (var sample in samples)
{
result += sample.time + ";" + sample.altitude + ";" + sample.velocity + ";" + sample.acceleration + ";" + sample.mass + ";" + sample.throttle + "\n";
}
Debug.Log(result);
}
#endif
}
