//predict the reentry trajectory. uses the fourth order Runge-Kutta numerical integration scheme
protected LandingPrediction simulateReentryRK4(Vector3d startPos, Vector3d startVel, double startTime, double dt, bool recurse)
{
LandingPrediction result = new LandingPrediction();
//should change this to also account for hyperbolic orbits where we have passed periapsis
//should also change this to use the orbit giv en by the parameters
if (part.vessel.orbit.PeA > part.vessel.mainBody.maxAtmosphereAltitude)
{
result.outcome = LandingPrediction.Outcome.NO_REENTRY;
return result;
}
//use the known orbit in vacuum to find the position and velocity when we first hit the atmosphere
//or start decelerating with thrust:
AROrbit freefallTrajectory = new AROrbit(startPos, startVel, startTime, part.vessel.mainBody);
double initialT = projectFreefallEndTime(freefallTrajectory, startTime);
//a hack to detect improperly initialized stuff and not try to do the simulation
if (double.IsNaN(initialT))
{
result.outcome = LandingPrediction.Outcome.NO_REENTRY;
return result;
}
Vector3d pos = freefallTrajectory.positionAtTime(initialT);
Vector3d vel = freefallTrajectory.velocityAtTime(initialT);
double initialAltitude = FlightGlobals.getAltitudeAtPos(pos);
Vector3d initialPos = new Vector3d(pos.x, pos.y, pos.z);
Vector3d initialVel = new Vector3d(vel.x, vel.y, vel.z); ;
double dragCoeffOverMass = vesselState.massDrag / vesselState.mass;
//now integrate the equations of motion until we hit the surface or we exit the atmosphere again:
double t = initialT;
result.maxGees = 0;
bool beenInAtmosphere = false;
while (true)
{
//one time step of RK4:
Vector3d dv1 = dt * ARUtils.computeTotalAccel(pos, vel, dragCoeffOverMass, part.vessel.mainBody);
Vector3d dx1 = dt * vel;
Vector3d dv2 = dt * ARUtils.computeTotalAccel(pos + 0.5 * dx1, vel + 0.5 * dv1, dragCoeffOverMass, part.vessel.mainBody);
Vector3d dx2 = dt * (vel + 0.5 * dv1);
Vector3d dv3 = dt * ARUtils.computeTotalAccel(pos + 0.5 * dx2, vel + 0.5 * dv2, dragCoeffOverMass, part.vessel.mainBody);
Vector3d dx3 = dt * (vel + 0.5 * dv2);
Vector3d dv4 = dt * ARUtils.computeTotalAccel(pos + dx3, vel + dv3, dragCoeffOverMass, part.vessel.mainBody);
Vector3d dx4 = dt * (vel + dv3);
Vector3d dx = (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6.0;
Vector3d dv = (dv1 + 2 * dv2 + 2 * dv3 + dv4) / 6.0;
pos += dx;
vel += dv;
t += dt;
double altitudeASL = FlightGlobals.getAltitudeAtPos(pos);
if (altitudeASL < part.vessel.mainBody.maxAtmosphereAltitude) beenInAtmosphere = true;
//We will thrust to reduce our speed if it is too high. However we have to
//be aware that it's the *surface* frame speed that is controlled.
Vector3d surfaceVel = vel - part.vessel.mainBody.getRFrmVel(pos);
double maxSurfaceVel = decelerationSpeed(altitudeASL, vesselState.maxThrustAccel, part.vessel.mainBody);
if (altitudeASL > decelerationEndAltitudeASL && surfaceVel.magnitude > maxSurfaceVel)
{
surfaceVel = maxSurfaceVel * surfaceVel.normalized;
vel = surfaceVel + part.vessel.mainBody.getRFrmVel(pos);
}
//during reentry the gees you feel come from drag:
double dragAccel = ARUtils.computeDragAccel(pos, vel, dragCoeffOverMass, part.vessel.mainBody).magnitude;
result.maxGees = Math.Max(dragAccel / 9.81, result.maxGees);
//detect landing
if (altitudeASL < decelerationEndAltitudeASL)
{
result.outcome = LandingPrediction.Outcome.LANDED;
result.landingLatitude = part.vessel.mainBody.GetLatitude(pos);
result.landingLongitude = part.vessel.mainBody.GetLongitude(pos);
result.landingLongitude -= (t - vesselState.time) * (360.0 / (part.vessel.mainBody.rotationPeriod)); //correct for planet rotation (360 degrees every 6 hours)
result.landingLongitude = ARUtils.clampDegrees(result.landingLongitude);
result.landingTime = t;
return result;
}
//detect the end of an aerobraking pass
if (beenInAtmosphere
&& part.vessel.mainBody.maxAtmosphereAltitude > 0
&& altitudeASL > part.vessel.mainBody.maxAtmosphereAltitude + 1000
&& Vector3d.Dot(vel, pos - part.vessel.mainBody.position) > 0)
{
if (part.vessel.orbit.PeA > 0 || !recurse)
{
result.outcome = LandingPrediction.Outcome.AEROBRAKED;
result.aerobrakeApoapsis = ARUtils.computeApoapsis(pos, vel, part.vessel.mainBody);
result.aerobrakePeriapsis = ARUtils.computePeriapsis(pos, vel, part.vessel.mainBody);
return result;
}
else
{
//continue suborbital trajectories to a landing
return simulateReentryRK4(pos, vel, t, dt, false);
}
}
//Don't tie up the CPU by running forever. Some real reentry trajectories will time out, but that's
//just too bad. It's possible to spend an arbitarily long time in the atmosphere before landing.
//For example, a circular orbit just inside the edge of the atmosphere will decay
//extremely slowly. So there's no reasonable timeout setting here that will simulate all valid reentry trajectories
//to their conclusion.
if (t > initialT + 1000)
{
result.outcome = LandingPrediction.Outcome.TIMED_OUT;
print("MechJeb landing simulation timed out:");
print("current ship altitude ASL = " + vesselState.altitudeASL);
print("freefall end altitude ASL = " + initialAltitude);
print("freefall end position = " + initialPos);
print("freefall end vel = " + initialVel);
print("timeout position = " + pos);
print("timeout altitude ASL = " + altitudeASL);
return result;
}
}
}
double predictSOISwitchTime(AROrbit transferOrbit, CelestialBody target, double startTime)
{
AROrbit targetOrbit = new AROrbit(target.position, target.orbit.GetVel(), vesselState.time, target.orbit.referenceBody);
double minSwitchTime = startTime;
double maxSwitchTime = 0;
for (double t = startTime; t < startTime + transferOrbit.period; )
{
Vector3d transferPos = transferOrbit.positionAtTime(t);
Vector3d targetPos = targetOrbit.positionAtTime(t);
if ((transferPos - targetPos).magnitude < target.sphereOfInfluence)
{
maxSwitchTime = t;
Vector3d transferVelS = transferOrbit.velocityAtTime(t);
Vector3d targetVelS = targetOrbit.velocityAtTime(t);
double transferPeTime = vesselState.time - (part.vessel.orbit.period - part.vessel.orbit.timeToPe);
double targetPeTime = vesselState.time - (target.orbit.period - target.orbit.timeToPe);
Vector3d kspTransferPosition = part.vessel.orbit.getPositionAt(t - transferPeTime);
Vector3d kspTargetPosition = target.orbit.getPositionAt(t - targetPeTime);
break;
}
else
{
minSwitchTime = t;
}
//advance time by a safe amount (so that we don't completely skip the SOI intersection)
Vector3d transferVel = transferOrbit.velocityAtTime(t);
Vector3d targetVel = targetOrbit.velocityAtTime(t);
Vector3d relativeVel = transferVel - targetVel;
t += 0.1 * target.sphereOfInfluence / relativeVel.magnitude;
}
if (maxSwitchTime == 0) return -1; //didn't intersect target's SOI
//do a binary search for the SOI entrance time:
while (maxSwitchTime - minSwitchTime > 1.0)
{
double testTime = (minSwitchTime + maxSwitchTime) / 2.0;
Vector3d transferPos = transferOrbit.positionAtTime(testTime);
Vector3d targetPos = targetOrbit.positionAtTime(testTime);
if ((transferPos - targetPos).magnitude < target.sphereOfInfluence) maxSwitchTime = testTime;
else minSwitchTime = testTime;
}
return (maxSwitchTime + minSwitchTime) / 2.0;
}
//In a landing, we are going to free fall some way and then either hit atmosphere or start
//decelerating with thrust. Until that happens we can project the orbit forward along a conic section.
//Given an orbit, this function figures out the time at which the free-fall phase will end
double projectFreefallEndTime(AROrbit offsetOrbit, double startTime)
{
Vector3d currentPosition = offsetOrbit.positionAtTime(startTime);
double currentAltitude = FlightGlobals.getAltitudeAtPos(currentPosition);
//check if we are already in the atmosphere or below the deceleration burn end altitude
if (currentAltitude < part.vessel.mainBody.maxAtmosphereAltitude || currentAltitude < decelerationEndAltitudeASL)
{
return vesselState.time;
}
Vector3d currentOrbitVelocity = offsetOrbit.velocityAtTime(startTime);
Vector3d currentSurfaceVelocity = currentOrbitVelocity - part.vessel.mainBody.getRFrmVel(currentPosition);
//check if we already should be decelerating or will be momentarily:
//that is, if our velocity is downward and our speed is close to the nominal deceleration speed
if (Vector3d.Dot(currentSurfaceVelocity, currentPosition - part.vessel.mainBody.position) < 0
&& currentSurfaceVelocity.magnitude > 0.9 * decelerationSpeed(currentAltitude, vesselState.maxThrustAccel, part.vessel.mainBody))
{
return vesselState.time;
}
//check if the orbit reenters at all
double timeToPe = offsetOrbit.timeToPeriapsis(startTime);
Vector3d periapsisPosition = offsetOrbit.positionAtTime(startTime + timeToPe);
if (FlightGlobals.getAltitudeAtPos(periapsisPosition) > part.vessel.mainBody.maxAtmosphereAltitude)
{
return startTime + timeToPe; //return the time of periapsis as a next best number
}
//determine time & velocity of reentry
double minReentryTime = startTime;
double maxReentryTime = startTime + timeToPe;
while (maxReentryTime - minReentryTime > 1.0)
{
double test = (maxReentryTime + minReentryTime) / 2.0;
Vector3d testPosition = offsetOrbit.positionAtTime(test);
double testAltitude = FlightGlobals.getAltitudeAtPos(testPosition);
Vector3d testOrbitVelocity = offsetOrbit.velocityAtTime(test);
Vector3d testSurfaceVelocity = testOrbitVelocity - part.vessel.mainBody.getRFrmVel(testPosition);
double testSpeed = testSurfaceVelocity.magnitude;
if (Vector3d.Dot(testSurfaceVelocity, testPosition - part.vessel.mainBody.position) < 0 &&
(testAltitude < part.vessel.mainBody.maxAtmosphereAltitude
|| testAltitude < decelerationEndAltitudeASL
|| testSpeed > 0.9 * decelerationSpeed(testAltitude, vesselState.maxThrustAccel, part.vessel.mainBody)))
{
maxReentryTime = test;
}
else
{
minReentryTime = test;
}
}
return (maxReentryTime + minReentryTime) / 2;
}
double predictPostTransferPeriapsis(Vector3d pos, Vector3d vel, double startTime, CelestialBody target)
{
AROrbit transferOrbit = new AROrbit(pos, vel, startTime, part.vessel.mainBody);
AROrbit targetOrbit = new AROrbit(target.position, target.orbit.GetVel(), vesselState.time, target.orbit.referenceBody);
double soiSwitchTime = predictSOISwitchTime(transferOrbit, target, startTime);
if (soiSwitchTime == -1) return -1;
Vector3d transferPos = transferOrbit.positionAtTime(soiSwitchTime);
Vector3d targetPos = targetOrbit.positionAtTime(soiSwitchTime);
Vector3d relativePos = transferPos - targetPos;
Vector3d transferVel = transferOrbit.velocityAtTime(soiSwitchTime);
Vector3d targetVel = targetOrbit.velocityAtTime(soiSwitchTime);
Vector3d relativeVel = transferVel - targetVel;
double pe = computePeriapsis(relativePos, relativeVel, target);
return pe;
}
