private Orbit CreateOrbitFromState(VesselState state)
{
var orbit = new Orbit();
orbit.UpdateFromStateVectors(SwapYZ(state.Position), SwapYZ(state.Velocity), state.ReferenceBody, state.Time);
var pars = new PatchedConics.SolverParameters
{
FollowManeuvers = false
};
PatchedConics.CalculatePatch(orbit, new Orbit(), state.Time, pars, null);
return(orbit);
}
private IEnumerable <bool> AddPatch(VesselState startingState, bool isActiveVessel)
{
if (isActiveVessel)
{
if (null == attachedVessel.patchedConicSolver)
{
UnityEngine.Debug.LogWarning("Trajectories: AddPatch() attempted when patchedConicsSolver is null; Skipping.");
yield break;
}
}
CelestialBody body = startingState.ReferenceBody;
var patch = new Patch
{
StartingState = startingState,
IsAtmospheric = false,
SpaceOrbit = startingState.StockPatch ?? CreateOrbitFromState(startingState)
};
patch.EndTime = patch.StartingState.Time + patch.SpaceOrbit.period;
// the flight plan does not always contain the first patches (before the first maneuver node),
// so we populate it with the current orbit and associated encounters etc.
var flightPlan = new List <Orbit>();
for (var orbit = attachedVessel.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
flightPlan.Add(orbit);
}
Orbit nextPatch = null;
if (startingState.StockPatch == null)
{
nextPatch = patch.SpaceOrbit.nextPatch;
}
else
{
int planIdx = flightPlan.IndexOf(startingState.StockPatch);
if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
{
nextPatch = flightPlan[planIdx + 1];
}
}
if (nextPatch != null)
{
patch.EndTime = nextPatch.StartUT;
}
double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);
if (!body.atmosphere)
{
maxAtmosphereAltitude = body.pqsController.mapMaxHeight;
}
double minAltitude = patch.SpaceOrbit.PeA;
if (patch.SpaceOrbit.timeToPe < 0 || patch.EndTime < startingState.Time + patch.SpaceOrbit.timeToPe)
{
minAltitude = Math.Min(
patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).magnitude,
patch.SpaceOrbit.getRelativePositionAtUT(patch.StartingState.Time + 1.0).magnitude
) - body.Radius;
}
if (minAltitude < maxAtmosphereAltitude)
{
double entryTime;
if (startingState.Position.magnitude <= body.Radius + maxAtmosphereAltitude)
{
// whole orbit is inside the atmosphere
entryTime = startingState.Time;
}
else
{
entryTime = FindOrbitBodyIntersection(
patch.SpaceOrbit,
startingState.Time, startingState.Time + patch.SpaceOrbit.timeToPe,
body.Radius + maxAtmosphereAltitude);
}
if (entryTime > startingState.Time + 0.1 || !body.atmosphere)
{
if (body.atmosphere)
{
// add the space patch before atmospheric entry
patch.EndTime = entryTime;
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
Position = SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(entryTime)),
ReferenceBody = body,
Time = entryTime,
Velocity = SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime))
};
yield break;
}
else
{
// the body has no atmosphere, so what we actually computed is the entry
// inside the "ground sphere" (defined by the maximal ground altitude)
// now we iterate until the inner ground sphere (minimal altitude), and
// check if we hit the ground along the way
double groundRangeExit = FindOrbitBodyIntersection(
patch.SpaceOrbit,
startingState.Time, startingState.Time + patch.SpaceOrbit.timeToPe,
body.Radius - maxAtmosphereAltitude);
if (groundRangeExit <= entryTime)
{
groundRangeExit = startingState.Time + patch.SpaceOrbit.timeToPe;
}
double iterationSize = (groundRangeExit - entryTime) / 100.0;
double t;
bool groundImpact = false;
for (t = entryTime; t < groundRangeExit; t += iterationSize)
{
Vector3d pos = patch.SpaceOrbit.getRelativePositionAtUT(t);
double groundAltitude = GetGroundAltitude(body, CalculateRotatedPosition(body, SwapYZ(pos), t))
+ body.Radius;
if (pos.magnitude < groundAltitude)
{
t -= iterationSize;
groundImpact = true;
break;
}
}
if (groundImpact)
{
patch.EndTime = t;
patch.RawImpactPosition = SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(t));
patch.ImpactPosition = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, t);
patch.ImpactVelocity = SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(t));
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else
{
// no impact, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextPatch != null)
{
AddPatch_outState = new VesselState
{
Position = SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime)),
Velocity = SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime)),
ReferenceBody = nextPatch == null ? body : nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
yield break;
}
else
{
AddPatch_outState = null;
yield break;
}
}
}
}
else
{
if (patch.StartingState.ReferenceBody != attachedVessel.mainBody)
{
// currently, we can't handle predictions for another body, so we stop
AddPatch_outState = null;
yield break;
}
// simulate atmospheric flight (drag and lift), until impact or atmosphere exit
// (typically for an aerobraking maneuver) assuming a constant angle of attack
patch.IsAtmospheric = true;
patch.StartingState.StockPatch = null;
// lower dt would be more accurate, but a tradeoff has to be found between performances and accuracy
double dt = Settings.IntegrationStepSize;
// some shallow entries can result in very long flight. For performances reasons,
// we limit the prediction duration
int maxIterations = (int)(60.0 * 60.0 / dt);
int chunkSize = 128;
// time between two consecutive stored positions (more intermediate positions are computed for better accuracy),
// also used for ground collision checks
double trajectoryInterval = 10.0;
var buffer = new List <Point[]>
{
new Point[chunkSize]
};
int nextPosIdx = 0;
SimulationState state;
state.position = SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(entryTime));
state.velocity = SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime));
// Initialize a patch with zero acceleration
Vector3d currentAccel = new Vector3d(0.0, 0.0, 0.0);
double currentTime = entryTime;
double lastPositionStoredUT = 0;
Vector3d lastPositionStored = new Vector3d();
bool hitGround = false;
int iteration = 0;
int incrementIterations = 0;
int minIterationsPerIncrement = maxIterations / Settings.MaxFramesPerPatch;
#region Acceleration Functor
// function that calculates the acceleration under current parmeters
Func <Vector3d, Vector3d, Vector3d> accelerationFunc = (position, velocity) =>
{
//Profiler.Start("accelerationFunc inside");
// gravity acceleration
double R_ = position.magnitude;
Vector3d accel_g = position * (-body.gravParameter / (R_ * R_ * R_));
// aero force
Vector3d vel_air = velocity - body.getRFrmVel(body.position + position);
double aoa = 0;
//Profiler.Start("GetForces");
Vector3d force_aero = aerodynamicModel_.GetForces(body, position, vel_air, aoa);
//Profiler.Stop("GetForces");
Vector3d accel = accel_g + force_aero / aerodynamicModel_.mass;
//Profiler.Stop("accelerationFunc inside");
return(accel);
};
#endregion
#region Integration Loop
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
{
yield return(false);
incrementIterations = 0;
}
double R = state.position.magnitude;
double altitude = R - body.Radius;
double atmosphereCoeff = altitude / maxAtmosphereAltitude;
if (hitGround ||
atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 ||
iteration == maxIterations || currentTime > patch.EndTime)
{
//Util.PostSingleScreenMessage("atmo force", "Atmospheric accumulated force: " + accumulatedForces.ToString("0.00"));
if (hitGround || atmosphereCoeff <= 0.0)
{
patch.RawImpactPosition = state.position;
patch.ImpactPosition = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, currentTime);
patch.ImpactVelocity = state.velocity;
}
patch.EndTime = Math.Min(currentTime, patch.EndTime);
int totalCount = (buffer.Count - 1) * chunkSize + nextPosIdx;
patch.AtmosphericTrajectory = new Point[totalCount];
int outIdx = 0;
foreach (var chunk in buffer)
{
foreach (var p in chunk)
{
if (outIdx == totalCount)
{
break;
}
patch.AtmosphericTrajectory[outIdx++] = p;
}
}
if (iteration == maxIterations)
{
ScreenMessages.PostScreenMessage("WARNING: trajectory prediction stopped, too many iterations");
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else if (atmosphereCoeff <= 0.0 || hitGround)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
Position = state.position,
Velocity = state.velocity,
ReferenceBody = body,
Time = patch.EndTime
};
yield break;
}
}
Vector3d lastAccel = currentAccel;
SimulationState lastState = state;
//Profiler.Start("IntegrationStep");
// Verlet integration (more precise than using the velocity)
// state = VerletStep(state, accelerationFunc, dt);
state = RK4Step(state, accelerationFunc, dt, out currentAccel);
currentTime += dt;
// KSP presumably uses Euler integration for position updates. Since RK4 is actually more precise than that,
// we try to reintroduce an approximation of the error.
// The local truncation error for euler integration is:
// LTE = 1/2 * h^2 * y''(t)
// https://en.wikipedia.org/wiki/Euler_method#Local_truncation_error
//
// For us,
// h is the time step of the outer simulation (KSP), which is the physics time step
// y''(t) is the difference of the velocity/acceleration divided by the physics time step
state.position += 0.5 * TimeWarp.fixedDeltaTime * currentAccel * dt;
state.velocity += 0.5 * TimeWarp.fixedDeltaTime * (currentAccel - lastAccel);
//Profiler.Stop("IntegrationStep");
// calculate gravity and aerodynamic force
Vector3d gravityAccel = lastState.position * (-body.gravParameter / (R * R * R));
Vector3d aerodynamicForce = (currentAccel - gravityAccel) / aerodynamicModel_.mass;
// acceleration in the vessel reference frame is acceleration - gravityAccel
maxAccelBackBuffer_ = Math.Max(
(float)(aerodynamicForce.magnitude / aerodynamicModel_.mass),
maxAccelBackBuffer_);
#region Impact Calculation
//Profiler.Start("AddPatch#impact");
double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
if (currentTime >= lastPositionStoredUT + interval)
{
double groundAltitude = GetGroundAltitude(body, CalculateRotatedPosition(body, state.position, currentTime));
if (lastPositionStoredUT > 0)
{
// check terrain collision, to detect impact on mountains etc.
Vector3 rayOrigin = lastPositionStored;
Vector3 rayEnd = state.position;
double absGroundAltitude = groundAltitude + body.Radius;
if (absGroundAltitude > rayEnd.magnitude)
{
hitGround = true;
float coeff = Math.Max(0.01f, (float)((absGroundAltitude - rayOrigin.magnitude)
/ (rayEnd.magnitude - rayOrigin.magnitude)));
state.position = rayEnd * coeff + rayOrigin * (1.0f - coeff);
currentTime = currentTime * coeff + lastPositionStoredUT * (1.0f - coeff);
}
}
lastPositionStoredUT = currentTime;
if (nextPosIdx == chunkSize)
{
buffer.Add(new Point[chunkSize]);
nextPosIdx = 0;
}
Vector3d nextPos = state.position;
if (Settings.BodyFixedMode)
{
nextPos = CalculateRotatedPosition(body, nextPos, currentTime);
}
buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
buffer.Last()[nextPosIdx].orbitalVelocity = state.velocity;
buffer.Last()[nextPosIdx].groundAltitude = (float)groundAltitude;
buffer.Last()[nextPosIdx].time = currentTime;
buffer.Last()[nextPosIdx++].pos = nextPos;
lastPositionStored = state.position;
}
//Profiler.Stop("AddPatch#impact");
#endregion
}
#endregion
}
}
else
{
// no atmospheric entry, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextPatch != null)
{
AddPatch_outState = new VesselState
{
Position = SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime)),
Velocity = SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime)),
ReferenceBody = nextPatch == null ? body : nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
yield break;
}
else
{
AddPatch_outState = null;
yield break;
}
}
}
private IEnumerable <bool> ComputeTrajectoryIncrement(Vessel vessel, DescentProfile profile)
{
// create or update aerodynamic model
if (aerodynamicModel_ == null || !aerodynamicModel_.isValidFor(vessel, vessel.mainBody))
{
aerodynamicModel_ = AerodynamicModelFactory.GetModel(vessel, vessel.mainBody);
}
else
{
aerodynamicModel_.IncrementalUpdate();
}
// create new VesselState from vessel, or null if it's on the ground
var state = new VesselState(vessel);
// iterate over patches until MaxPatchCount is reached
for (int patchIdx = 0; patchIdx < Settings.MaxPatchCount; ++patchIdx)
{
// stop if we don't have a vessel state
if (state == null)
{
state = new VesselState(vessel);
}
// If we spent more time in this calculation than allowed, pause until the next frame
if (incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
{
yield return(false);
}
// if we have a patched conics solver, check for maneuver nodes
if (null != attachedVessel.patchedConicSolver)
{
// search through maneuver nodes of the vessel
var maneuverNodes = attachedVessel.patchedConicSolver.maneuverNodes;
foreach (var node in maneuverNodes)
{
// if the maneuver node time corresponds to the end time of the last patch
if (node.UT == state.Time)
{
// add the velocity change of the burn to the velocity of the last patch
state.Velocity += node.GetBurnVector(CreateOrbitFromState(state));
break;
}
}
// Add one patch, then pause execution after every patch
foreach (bool result in AddPatch(state, true))
{
yield return(false);
}
}
else
{
// Add one patch, then pause execution after every patch
foreach (bool result in AddPatch(state, false))
{
yield return(false);
}
}
state = AddPatch_outState;
}
}
