private static Vector3d CalcReference()
{
if (!Trajectories.IsVesselAttached || TargetProfile.Body == null)
{
return(Vector3d.zero);
}
double plannedAngleOfAttack = (double)DescentProfile.GetAngleOfAttack(TargetProfile.Body, position, velocity);
return(velocity.normalized * Math.Cos(plannedAngleOfAttack) + Vector3d.Cross(vel_right, velocity).normalized *Math.Sin(plannedAngleOfAttack));
}
private IEnumerable <bool> AddPatch(VesselState startingState)
{
if (null == AttachedVessel.patchedConicSolver)
{
AttachedVessel.AttachPatchedConicsSolver();
}
CelestialBody body = startingState.ReferenceBody;
Patch 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.
List <Orbit> flightPlan = new List <Orbit>();
for (Orbit orbit = AttachedVessel.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
if (AttachedVessel.patchedConicSolver.flightPlan.Contains(orbit))
{
break;
}
flightPlan.Add(orbit);
}
foreach (Orbit orbit in AttachedVessel.patchedConicSolver.flightPlan)
{
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 = patch.SpaceOrbit.getRelativePositionAtUT(entryTime).SwapYZ(),
ReferenceBody = body,
Time = entryTime,
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime).SwapYZ()
};
}
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, pos.SwapYZ(), t))
+ body.Radius;
if (pos.magnitude < groundAltitude)
{
t -= iterationSize;
groundImpact = true;
break;
}
}
if (groundImpact)
{
patch.EndTime = t;
patch.RawImpactPosition = patch.SpaceOrbit.getRelativePositionAtUT(t).SwapYZ();
patch.ImpactPosition = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, t);
patch.ImpactVelocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(t).SwapYZ();
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
}
else
{
// no impact, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextPatch != null)
{
AddPatch_outState = new VesselState
{
Position = patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).SwapYZ(),
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime).SwapYZ(),
ReferenceBody = nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
}
else
{
AddPatch_outState = null;
}
}
}
}
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 trade-off 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;
List <Point[]> buffer = new List <Point[]>
{
new Point[chunkSize]
};
int nextPosIdx = 0;
SimulationState state;
state.position = patch.SpaceOrbit.getRelativePositionAtUT(entryTime).SwapYZ();
state.velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime).SwapYZ();
// 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 parameters
Vector3d AccelerationFunc(Vector3d position, Vector3d 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 = DescentProfile.GetAngleOfAttack(body, position, vel_air) ?? 0d;
Profiler.Start("GetForces");
Vector3d force_aero = aerodynamicModel_.GetForces(body, position, vel_air, aoa);
Profiler.Stop("GetForces");
Vector3d accel = accel_g + force_aero / aerodynamicModel_.Mass;
accel += API.HandleAccel(AttachedVessel, body, position, velocity, aoa);
Profiler.Stop("accelerationFunc inside");
return(accel);
}
#endregion
#region Integration Loop
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && Util.ElapsedMilliseconds(increment_time) > MAX_INCREMENT_TIME)
{
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 (Point[] chunk in buffer)
{
foreach (Point 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;
}
if (atmosphereCoeff <= 0.0 || hitGround)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
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 = patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).SwapYZ(),
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime).SwapYZ(),
ReferenceBody = nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
}
else
{
AddPatch_outState = null;
}
}
}
private IEnumerable <bool> AddPatch(VesselState startingState, DescentProfile profile)
{
if (null == vessel_.patchedConicSolver)
{
UnityEngine.Debug.LogWarning("Trajectories: AddPatch() attempted when patchedConicsSolver is null; Skipping.");
yield break;
}
CelestialBody body = startingState.referenceBody;
var patch = new Patch();
patch.startingState = startingState;
patch.isAtmospheric = false;
patch.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 = vessel_.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
if (vessel_.patchedConicSolver.flightPlan.Contains(orbit))
{
break;
}
flightPlan.Add(orbit);
}
foreach (var orbit in vessel_.patchedConicSolver.flightPlan)
{
flightPlan.Add(orbit);
}
Orbit nextStockPatch = null;
if (startingState.stockPatch != null)
{
int planIdx = flightPlan.IndexOf(startingState.stockPatch);
if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
{
nextStockPatch = flightPlan[planIdx + 1];
}
}
if (nextStockPatch != null)
{
patch.endTime = nextStockPatch.StartUT;
}
double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);
double minAltitude = patch.spaceOrbit.PeA;
if (patch.endTime < startingState.time + patch.spaceOrbit.timeToPe)
{
minAltitude = patch.spaceOrbit.getRelativePositionAtUT(patch.endTime).magnitude;
}
if (minAltitude < maxAtmosphereAltitude)
{
double entryTime;
if (startingState.position.magnitude <= body.Radius + maxAtmosphereAltitude)
{
// whole orbit is inside the atmosphere
entryTime = startingState.time;
}
else
{
// binary search of entry time in atmosphere
// I guess an analytic solution could be found, but I'm too lazy to search it
double from = startingState.time;
double to = from + patch.spaceOrbit.timeToPe;
int loopCount = 0;
while (to - from > 0.1)
{
++loopCount;
if (loopCount > 1000)
{
UnityEngine.Debug.Log("WARNING: infinite loop? (Trajectories.Trajectory.AddPatch, atmosphere limit search)");
++errorCount_;
break;
}
double middle = (from + to) * 0.5;
if (patch.spaceOrbit.getRelativePositionAtUT(middle).magnitude < body.Radius + maxAtmosphereAltitude)
{
to = middle;
}
else
{
from = middle;
}
}
entryTime = to;
}
if (entryTime > startingState.time + 0.1)
{
// add the space patch before atmospheric entry
patch.endTime = entryTime;
if (body.atmosphere)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)),
referenceBody = body,
time = entryTime,
velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime))
};
yield break;
}
else
{
// the body has no atmosphere, so what we actually computed is the impact on the body surface
// now, go back in time until the impact point is above the ground to take ground height in account
// we assume the ground is horizontal around the impact position
double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)), entryTime)) + body.Radius;
double iterationSize = 1.0;
while (entryTime > startingState.time + iterationSize && patch.spaceOrbit.getRelativePositionAtUT(entryTime).magnitude < groundAltitude)
{
entryTime -= iterationSize;
}
patch.endTime = entryTime;
patch.rawImpactPosition = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, entryTime);
patch.impactVelocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
}
else
{
if (patch.startingState.referenceBody != vessel_.mainBody)
{
// in current aerodynamic prediction code, we can't handle predictions for another body, so we stop here
AddPatch_outState = null;
yield break;
}
// simulate atmospheric flight (drag and lift), until landing (more likely to be a crash as we don't predict user piloting) or atmosphere exit (typically for an aerobraking maneuver)
// the simulation assumes a constant angle of attack
patch.isAtmospheric = true;
patch.startingState.stockPatch = null;
double dt = 0.1; // lower dt would be more accurate, but a tradeoff has to be found between performances and accuracy
int maxIterations = (int)(30.0 * 60.0 / dt); // some shallow entries can result in very long flight, for performances reasons, we limit the prediction duration
int chunkSize = 128;
double trajectoryInterval = 10.0; // time between two consecutive stored positions (more intermediate positions are computed for better accuracy), also used for ground collision checks
var buffer = new List <Point[]>();
buffer.Add(new Point[chunkSize]);
int nextPosIdx = 0;
Vector3d pos = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
Vector3d vel = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
Vector3d prevPos = pos - vel * dt;
//Util.PostSingleScreenMessage("initial vel", "initial vel = " + vel);
double currentTime = entryTime;
double lastPositionStoredUT = 0;
Vector3d lastPositionStored = new Vector3d();
bool hitGround = false;
int iteration = 0;
int incrementIterations = 0;
int minIterationsPerIncrement = maxIterations / Settings.fetch.MaxFramesPerPatch;
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
{
yield return(false);
incrementIterations = 0;
}
double R = pos.magnitude;
double altitude = R - body.Radius;
double atmosphereCoeff = altitude / maxAtmosphereAltitude;
if (hitGround || atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 || iteration == maxIterations || currentTime > patch.endTime)
{
if (hitGround || atmosphereCoeff <= 0.0)
{
patch.rawImpactPosition = pos;
patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, currentTime);
patch.impactVelocity = vel;
}
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 = pos,
velocity = vel,
referenceBody = body,
time = patch.endTime
};
yield break;
}
}
Vector3d gravityAccel = pos * (-body.gravParameter / (R * R * R));
//Util.PostSingleScreenMessage("prediction vel", "prediction vel = " + vel);
Vector3d airVelocity = vel - body.getRFrmVel(body.position + pos);
double angleOfAttack = profile.GetAngleOfAttack(body, pos, airVelocity);
Vector3d aerodynamicForce = aerodynamicModel_.GetForces(body, pos, airVelocity, angleOfAttack);
Vector3d acceleration = gravityAccel + aerodynamicForce / aerodynamicModel_.mass;
// acceleration in the vessel reference frame is acceleration - gravityAccel
maxAccelBackBuffer_ = Math.Max((float)(aerodynamicForce.magnitude / aerodynamicModel_.mass), maxAccelBackBuffer_);
//vel += acceleration * dt;
//pos += vel * dt;
// Verlet integration (more precise than using the velocity)
Vector3d ppos = prevPos;
prevPos = pos;
pos = pos + pos - ppos + acceleration * (dt * dt);
vel = (pos - prevPos) / dt;
currentTime += dt;
double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
if (currentTime >= lastPositionStoredUT + interval)
{
double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, pos, currentTime));
if (lastPositionStoredUT > 0)
{
// check terrain collision, to detect impact on mountains etc.
Vector3 rayOrigin = lastPositionStored;
Vector3 rayEnd = pos;
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)));
pos = 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 = pos;
if (Settings.fetch.BodyFixedMode)
{
nextPos = calculateRotatedPosition(body, nextPos, currentTime);
}
buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
buffer.Last()[nextPosIdx].orbitalVelocity = vel;
buffer.Last()[nextPosIdx].groundAltitude = (float)groundAltitude;
buffer.Last()[nextPosIdx].time = currentTime;
buffer.Last()[nextPosIdx++].pos = nextPos;
lastPositionStored = pos;
}
}
}
}
else
{
// no atmospheric entry, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextStockPatch != null)
{
AddPatch_outState = new VesselState
{
position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(patch.endTime)),
velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(patch.endTime)),
referenceBody = nextStockPatch == null ? body : nextStockPatch.referenceBody,
time = patch.endTime,
stockPatch = nextStockPatch
};
yield break;
}
else
{
AddPatch_outState = null;
yield break;
}
}
}
private IEnumerable<bool> AddPatch(VesselState startingState, DescentProfile profile)
{
if (null == vessel_.patchedConicSolver)
{
UnityEngine.Debug.LogWarning("Trajectories: AddPatch() attempted when patchedConicsSolver is null; Skipping.");
yield break;
}
CelestialBody body = startingState.referenceBody;
var patch = new Patch();
patch.startingState = startingState;
patch.isAtmospheric = false;
patch.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 = vessel_.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
if (vessel_.patchedConicSolver.flightPlan.Contains(orbit))
break;
flightPlan.Add(orbit);
}
foreach (var orbit in vessel_.patchedConicSolver.flightPlan)
{
flightPlan.Add(orbit);
}
Orbit nextStockPatch = null;
if (startingState.stockPatch != null)
{
int planIdx = flightPlan.IndexOf(startingState.stockPatch);
if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
{
nextStockPatch = flightPlan[planIdx + 1];
}
}
if (nextStockPatch != null)
{
patch.endTime = nextStockPatch.StartUT;
}
double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);
double minAltitude = patch.spaceOrbit.PeA;
if (patch.endTime < startingState.time + patch.spaceOrbit.timeToPe)
{
minAltitude = patch.spaceOrbit.getRelativePositionAtUT(patch.endTime).magnitude;
}
if (minAltitude < maxAtmosphereAltitude)
{
double entryTime;
if (startingState.position.magnitude <= body.Radius + maxAtmosphereAltitude)
{
// whole orbit is inside the atmosphere
entryTime = startingState.time;
}
else
{
// binary search of entry time in atmosphere
// I guess an analytic solution could be found, but I'm too lazy to search it
double from = startingState.time;
double to = from + patch.spaceOrbit.timeToPe;
int loopCount = 0;
while (to - from > 0.1)
{
++loopCount;
if (loopCount > 1000)
{
UnityEngine.Debug.Log("WARNING: infinite loop? (Trajectories.Trajectory.AddPatch, atmosphere limit search)");
++errorCount_;
break;
}
double middle = (from + to) * 0.5;
if (patch.spaceOrbit.getRelativePositionAtUT(middle).magnitude < body.Radius + maxAtmosphereAltitude)
{
to = middle;
}
else
{
from = middle;
}
}
entryTime = to;
}
if (entryTime > startingState.time + 0.1)
{
// add the space patch before atmospheric entry
patch.endTime = entryTime;
if (body.atmosphere)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)),
referenceBody = body,
time = entryTime,
velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime))
};
yield break;
}
else
{
// the body has no atmosphere, so what we actually computed is the impact on the body surface
// now, go back in time until the impact point is above the ground to take ground height in account
// we assume the ground is horizontal around the impact position
double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)), entryTime)) + body.Radius;
double iterationSize = 1.0;
while (entryTime > startingState.time + iterationSize && patch.spaceOrbit.getRelativePositionAtUT(entryTime).magnitude < groundAltitude)
entryTime -= iterationSize;
patch.endTime = entryTime;
patch.rawImpactPosition = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, entryTime);
patch.impactVelocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
}
else
{
if (patch.startingState.referenceBody != vessel_.mainBody)
{
// in current aerodynamic prediction code, we can't handle predictions for another body, so we stop here
AddPatch_outState = null;
yield break;
}
// simulate atmospheric flight (drag and lift), until landing (more likely to be a crash as we don't predict user piloting) or atmosphere exit (typically for an aerobraking maneuver)
// the simulation assumes a constant angle of attack
patch.isAtmospheric = true;
patch.startingState.stockPatch = null;
double dt = 0.1; // lower dt would be more accurate, but a tradeoff has to be found between performances and accuracy
int maxIterations = (int)(30.0 * 60.0 / dt); // some shallow entries can result in very long flight, for performances reasons, we limit the prediction duration
int chunkSize = 128;
double trajectoryInterval = 10.0; // time between two consecutive stored positions (more intermediate positions are computed for better accuracy), also used for ground collision checks
var buffer = new List<Point[]>();
buffer.Add(new Point[chunkSize]);
int nextPosIdx = 0;
Vector3d pos = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
Vector3d vel = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
Vector3d prevPos = pos - vel * dt;
//Util.PostSingleScreenMessage("initial vel", "initial vel = " + vel);
double currentTime = entryTime;
double lastPositionStoredUT = 0;
Vector3d lastPositionStored = new Vector3d();
bool hitGround = false;
int iteration = 0;
int incrementIterations = 0;
int minIterationsPerIncrement = maxIterations / Settings.fetch.MaxFramesPerPatch;
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
{
yield return false;
incrementIterations = 0;
}
double R = pos.magnitude;
double altitude = R - body.Radius;
double atmosphereCoeff = altitude / maxAtmosphereAltitude;
if (hitGround || atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 || iteration == maxIterations || currentTime > patch.endTime)
{
if (hitGround || atmosphereCoeff <= 0.0)
{
patch.rawImpactPosition = pos;
patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, currentTime);
patch.impactVelocity = vel;
}
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 = pos,
velocity = vel,
referenceBody = body,
time = patch.endTime
};
yield break;
}
}
Vector3d gravityAccel = pos * (-body.gravParameter / (R * R * R));
//Util.PostSingleScreenMessage("prediction vel", "prediction vel = " + vel);
Vector3d airVelocity = vel - body.getRFrmVel(body.position + pos);
double angleOfAttack = profile.GetAngleOfAttack(body, pos, airVelocity);
Vector3d aerodynamicForce = aerodynamicModel_.GetForces(body, pos, airVelocity, angleOfAttack);
Vector3d acceleration = gravityAccel + aerodynamicForce / aerodynamicModel_.mass;
// acceleration in the vessel reference frame is acceleration - gravityAccel
maxAccelBackBuffer_ = Math.Max((float) (aerodynamicForce.magnitude / aerodynamicModel_.mass), maxAccelBackBuffer_);
//vel += acceleration * dt;
//pos += vel * dt;
// Verlet integration (more precise than using the velocity)
Vector3d ppos = prevPos;
prevPos = pos;
pos = pos + pos - ppos + acceleration * (dt * dt);
vel = (pos - prevPos) / dt;
currentTime += dt;
double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
if (currentTime >= lastPositionStoredUT + interval)
{
double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, pos, currentTime));
if (lastPositionStoredUT > 0)
{
// check terrain collision, to detect impact on mountains etc.
Vector3 rayOrigin = lastPositionStored;
Vector3 rayEnd = pos;
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)));
pos = 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 = pos;
if (Settings.fetch.BodyFixedMode)
{
nextPos = calculateRotatedPosition(body, nextPos, currentTime);
}
buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
buffer.Last()[nextPosIdx].orbitalVelocity = vel;
buffer.Last()[nextPosIdx].groundAltitude = (float)groundAltitude;
buffer.Last()[nextPosIdx].time = currentTime;
buffer.Last()[nextPosIdx++].pos = nextPos;
lastPositionStored = pos;
}
}
}
}
else
{
// no atmospheric entry, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextStockPatch != null)
{
AddPatch_outState = new VesselState
{
position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(patch.endTime)),
velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(patch.endTime)),
referenceBody = nextStockPatch == null ? body : nextStockPatch.referenceBody,
time = patch.endTime,
stockPatch = nextStockPatch
};
yield break;
}
else
{
AddPatch_outState = null;
yield break;
}
}
}
private static Vector2d GetCorrection()
{
if (!Trajectories.IsVesselAttached)
{
return(Vector2d.zero);
}
Vector3d? targetPosition = TargetProfile.WorldPosition;
CelestialBody body = TargetProfile.Body;
if (!targetPosition.HasValue || patch == null || !patch.ImpactPosition.HasValue || patch.StartingState.ReferenceBody != body || !patch.IsAtmospheric)
{
return(Vector2d.zero);
}
// Get impact position, or, if some point over the trajectory has not enough clearance, smoothly interpolate to that point depending on how much clearance is missing
Vector3d impactPosition = patch.ImpactPosition.Value;
foreach (Trajectory.Point p in patch.AtmosphericTrajectory)
{
double neededClearance = 600.0d;
double missingClearance = neededClearance - (p.pos.magnitude - body.Radius - p.groundAltitude);
if (missingClearance > 0.0d)
{
if (Vector3d.Distance(p.pos, patch.RawImpactPosition.Value) > 3000.0d)
{
double coeff = missingClearance / neededClearance;
Vector3d rotatedPos = p.pos;
if (!Settings.BodyFixedMode)
{
rotatedPos = Trajectory.CalculateRotatedPosition(body, p.pos, p.time);
}
impactPosition = impactPosition * (1.0d - coeff) + rotatedPos * coeff;
}
break;
}
}
Vector3d right = Vector3d.Cross(patch.ImpactVelocity.Value, impactPosition).normalized;
Vector3d behind = Vector3d.Cross(right, impactPosition).normalized;
Vector3d offset = targetPosition.Value - impactPosition;
Vector2d offsetDir = new Vector2d(Vector3d.Dot(right, offset), Vector3d.Dot(behind, offset));
offsetDir *= 0.00005d; // 20km <-> 1 <-> 45° (this is purely indicative, no physical meaning, it would be very complicated to compute an actual correction angle as it depends on the spacecraft behavior in the atmosphere ; a small angle will suffice for a plane, but even a big angle might do almost nothing for a rocket)
Vector3d pos = Trajectories.AttachedVessel.GetWorldPos3D() - body.position;
Vector3d vel = Trajectories.AttachedVessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity
double plannedAngleOfAttack = (double)DescentProfile.GetAngleOfAttack(body, pos, vel);
if (plannedAngleOfAttack < Util.HALF_PI)
{
offsetDir.y = -offsetDir.y; // behavior is different for prograde or retrograde entry
}
double maxCorrection = 1.0d;
offsetDir.x = Util.Clamp(offsetDir.x, -maxCorrection, maxCorrection);
offsetDir.y = Util.Clamp(offsetDir.y, -maxCorrection, maxCorrection);
return(offsetDir);
}
