//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;
}
//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;
}
}
}
void driveDeorbitBurn(FlightCtrlState s)
{
//compute the desired velocity after deorbiting. we aim for a trajectory that
// a) has the same vertical speed as our current trajectory
// b) has a horizontal speed that will give it a periapsis of -10% of the body's radius
// c) has a heading that points toward where the target will be at the end of free-fall, accounting for planetary rotation
double horizontalDV = orbitOper.deltaVToChangePeriapsis(-0.1 * part.vessel.mainBody.Radius);
horizontalDV *= Math.Sign(-0.1 * part.vessel.mainBody.Radius - part.vessel.orbit.PeA);
Vector3d currentHorizontal = Vector3d.Exclude(vesselState.up, vesselState.velocityVesselOrbit).normalized;
Vector3d forwardDeorbitVelocity = vesselState.velocityVesselOrbit + horizontalDV * currentHorizontal;
AROrbit forwardDeorbitTrajectory = new AROrbit(vesselState.CoM, forwardDeorbitVelocity, vesselState.time, part.vessel.mainBody);
double freefallTime = projectFreefallEndTime(forwardDeorbitTrajectory, vesselState.time) - vesselState.time;
double planetRotationDuringFreefall = 360 * freefallTime / part.vessel.mainBody.rotationPeriod;
Vector3d currentTargetRadialVector = part.vessel.mainBody.GetRelSurfacePosition(targetLatitude, targetLongitude, 0);
Quaternion freefallPlanetRotation = Quaternion.AngleAxis((float)planetRotationDuringFreefall, part.vessel.mainBody.angularVelocity);
Vector3d freefallEndTargetRadialVector = freefallPlanetRotation * currentTargetRadialVector;
Vector3d currentTargetPosition = part.vessel.mainBody.position + part.vessel.mainBody.Radius * part.vessel.mainBody.GetSurfaceNVector(targetLatitude, targetLongitude);
Vector3d freefallEndTargetPosition = part.vessel.mainBody.position + freefallEndTargetRadialVector;
Vector3d freefallEndHorizontalToTarget = Vector3d.Exclude(vesselState.up, freefallEndTargetPosition - vesselState.CoM).normalized;
double currentHorizontalSpeed = Vector3d.Exclude(vesselState.up, vesselState.velocityVesselOrbit).magnitude;
double finalHorizontalSpeed = currentHorizontalSpeed + horizontalDV;
double currentVerticalSpeed = Vector3d.Dot(vesselState.up, vesselState.velocityVesselOrbit);
Vector3d currentRadialVector = vesselState.CoM - part.vessel.mainBody.position;
Vector3d currentOrbitNormal = Vector3d.Cross(currentRadialVector, vesselState.velocityVesselOrbit);
double targetAngleToOrbitNormal = Math.Abs(Vector3d.Angle(currentOrbitNormal, freefallEndTargetRadialVector));
targetAngleToOrbitNormal = Math.Min(targetAngleToOrbitNormal, 180 - targetAngleToOrbitNormal);
double targetAheadAngle = Math.Abs(Vector3d.Angle(currentRadialVector, freefallEndTargetRadialVector));
double planeChangeAngle = Math.Abs(Vector3d.Angle(currentHorizontal, freefallEndHorizontalToTarget));
if (!deorbiting)
{
if (targetAngleToOrbitNormal < 10
|| (targetAheadAngle < 90 && targetAheadAngle > 60 && planeChangeAngle < 90)) deorbiting = true;
}
if (deorbiting)
{
if (TimeWarp.CurrentRate > TimeWarp.MaxPhysicsRate) core.warpMinimum(this);
Vector3d desiredVelocity = finalHorizontalSpeed * freefallEndHorizontalToTarget + currentVerticalSpeed * vesselState.up;
Vector3d velocityChange = desiredVelocity - vesselState.velocityVesselOrbit;
if (velocityChange.magnitude < 2.0) landStep = LandStep.COURSE_CORRECTIONS;
core.attitudeTo(velocityChange.normalized, MechJebCore.AttitudeReference.INERTIAL, this);
if (core.attitudeAngleFromTarget() < 5) s.mainThrottle = 1.0F;
else s.mainThrottle = 0.0F;
landStatusString = "Executing deorbit burn";
}
else
{
//if we don't want to deorbit but we're already on a reentry trajectory, we can't wait until the ideal point
//in the orbit to deorbt; we already have deorbited.
if (part.vessel.orbit.ApA < part.vessel.mainBody.maxAtmosphereAltitude)
{
landStep = LandStep.COURSE_CORRECTIONS;
}
else
{
s.mainThrottle = 0.0F;
core.attitudeTo(Vector3d.back, MechJebCore.AttitudeReference.ORBIT, this);
core.warpIncrease(this);
}
landStatusString = "Moving to deorbit burn point";
}
}
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;
}
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;
}
double deltaVToChangeApoapsis(double newApA)
{
double newApR = newApA + part.vessel.mainBody.Radius;
//don't bother with impossible maneuvers:
if (newApR < vesselState.radius) return 0.0;
//are we raising or lowering the periapsis?
double initialAp = new AROrbit(part.vessel).apoapsis();
if (initialAp < 0) initialAp = double.MaxValue;
bool raising = (newApR > initialAp);
Vector3d burnDirection = (raising ? 1 : -1) * chooseThrustDirectionToRaiseApoapsis();
double minDeltaV = 0;
double maxDeltaV;
if (raising)
{
//put an upper bound on the required deltaV:
maxDeltaV = 0.25;
double ap = initialAp;
if (ap < 0) ap = double.MaxValue; //ap < 0 for hyperbolic orbits
while (ap < newApR)
{
maxDeltaV *= 2;
ap = new AROrbit(vesselState.CoM, vesselState.velocityVesselOrbit + maxDeltaV * burnDirection,
vesselState.time, part.vessel.mainBody).apoapsis();
if (ap < 0) ap = double.MaxValue;
}
}
else
{
//when lowering apoapsis, we burn retrograde, and max possible deltaV is total velocity
maxDeltaV = vesselState.speedOrbital;
}
//now do a binary search to find the needed delta-v
while (maxDeltaV - minDeltaV > 1.0)
{
double testDeltaV = (maxDeltaV + minDeltaV) / 2.0;
double testApoapsis = new AROrbit(vesselState.CoM, vesselState.velocityVesselOrbit + testDeltaV * burnDirection,
vesselState.time, part.vessel.mainBody).apoapsis();
if (testApoapsis < 0) testApoapsis = double.MaxValue;
if ((testApoapsis > newApR && raising) || (testApoapsis < newApR && !raising))
{
maxDeltaV = testDeltaV;
}
else
{
minDeltaV = testDeltaV;
}
}
return (maxDeltaV + minDeltaV) / 2;
}
public double deltaVToChangePeriapsis(double newPeA)
{
double newPeR = newPeA + part.vessel.mainBody.Radius;
//don't bother with impossible maneuvers:
if (newPeR > vesselState.radius) return 0.0;
//are we raising or lowering the periapsis?
bool raising = (newPeR > new AROrbit(part.vessel).periapsis());
Vector3d burnDirection = (raising ? 1 : -1) * chooseThrustDirectionToRaisePeriapsis();
double minDeltaV = 0;
double maxDeltaV;
if (raising)
{
//put an upper bound on the required deltaV:
maxDeltaV = 0.25;
while (new AROrbit(vesselState.CoM, vesselState.velocityVesselOrbit + maxDeltaV * burnDirection,
vesselState.time, part.vessel.mainBody).periapsis() < newPeR)
{
maxDeltaV *= 2;
}
}
else
{
//when lowering periapsis, we burn horizontally, and max possible deltaV is the deltaV required to kill all horizontal velocity
double horizontalV = Vector3d.Exclude(vesselState.up, vesselState.velocityVesselOrbit).magnitude;
maxDeltaV = horizontalV;
}
//now do a binary search to find the needed delta-v
while (maxDeltaV - minDeltaV > 1.0)
{
double testDeltaV = (maxDeltaV + minDeltaV) / 2.0;
double testPeriapsis = new AROrbit(vesselState.CoM, vesselState.velocityVesselOrbit + testDeltaV * burnDirection,
vesselState.time, part.vessel.mainBody).periapsis();
if ((testPeriapsis > newPeR && raising) || (testPeriapsis < newPeR && !raising))
{
maxDeltaV = testDeltaV;
}
else
{
minDeltaV = testDeltaV;
}
}
return (maxDeltaV + minDeltaV) / 2;
}
