//Estimate the delta-V of the correction burn that would be required to put us on
//course for the target
public Vector3d ComputeCourseCorrection(bool allowPrograde)
{
//actualLandingPosition is the predicted actual landing position
Vector3d actualLandingPosition = RotatedLandingSite - mainBody.position;
//orbitLandingPosition is the point where our current orbit intersects the planet
double endRadius = mainBody.Radius + DecelerationEndAltitude() - 100;
Vector3d orbitLandingPosition = orbit.SwappedRelativePositionAtUT(orbit.NextTimeOfRadius(vesselState.time, endRadius));
//convertOrbitToActual is a rotation that rotates orbitLandingPosition on actualLandingPosition
Quaternion convertOrbitToActual = Quaternion.FromToRotation(orbitLandingPosition, actualLandingPosition);
//Consider the effect small changes in the velocity in each of these three directions
Vector3d[] perturbationDirections = { vesselState.velocityVesselSurfaceUnit, vesselState.radialPlusSurface, vesselState.normalPlusSurface };
//Compute the effect burns in these directions would
//have on the landing position, where we approximate the landing position as the place
//the perturbed orbit would intersect the planet.
Vector3d[] deltas = new Vector3d[3];
for (int i = 0; i < 3; i++)
{
double perturbationDeltaV = 1; //warning: hard experience shows that setting this too low leads to bewildering bugs due to finite precision of Orbit functions
Orbit perturbedOrbit = orbit.PerturbedOrbit(vesselState.time, perturbationDeltaV * perturbationDirections[i]); //compute the perturbed orbit
double perturbedLandingTime;
if (perturbedOrbit.PeR < endRadius)
{
perturbedLandingTime = perturbedOrbit.NextTimeOfRadius(vesselState.time, endRadius);
}
else
{
perturbedLandingTime = perturbedOrbit.NextPeriapsisTime(vesselState.time);
}
Vector3d perturbedLandingPosition = perturbedOrbit.SwappedRelativePositionAtUT(perturbedLandingTime); //find where it hits the planet
Vector3d landingDelta = perturbedLandingPosition - orbitLandingPosition; //find the difference between that and the original orbit's intersection point
landingDelta = convertOrbitToActual * landingDelta; //rotate that difference vector so that we can now think of it as starting at the actual landing position
landingDelta = Vector3d.Exclude(actualLandingPosition, landingDelta); //project the difference vector onto the plane tangent to the actual landing position
deltas[i] = landingDelta / perturbationDeltaV; //normalize by the delta-V considered, so that deltas now has units of meters per (meter/second) [i.e., seconds]
}
//Now deltas stores the predicted offsets in landing position produced by each of the three perturbations.
//We now figure out the offset we actually want
//First we compute the target landing position. We have to convert the latitude and longitude of the target
//into a position. We can't just get the current position of those coordinates, because the planet will
//rotate during the descent, so we have to account for that.
Vector3d desiredLandingPosition = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0);
float bodyRotationAngleDuringDescent = (float)(360 * (prediction.endUT - vesselState.time) / mainBody.rotationPeriod);
Quaternion bodyRotationDuringFall = Quaternion.AngleAxis(bodyRotationAngleDuringDescent, mainBody.angularVelocity.normalized);
desiredLandingPosition = bodyRotationDuringFall * desiredLandingPosition;
Vector3d desiredDelta = desiredLandingPosition - actualLandingPosition;
desiredDelta = Vector3d.Exclude(actualLandingPosition, desiredDelta);
//Now desiredDelta gives the desired change in our actual landing position (projected onto a plane
//tangent to the actual landing position).
Vector3d downrangeDirection;
Vector3d downrangeDelta;
if (allowPrograde)
{
//Construct the linear combination of the prograde and radial+ perturbations
//that produces the largest effect on the landing position. The Math.Sign is to
//detect and handle the case where radial+ burns actually bring the landing sign closer
//(e.g. when we are traveling close to straight up)
downrangeDirection = (deltas[0].magnitude * perturbationDirections[0]
+ Math.Sign(Vector3d.Dot(deltas[0], deltas[1])) * deltas[1].magnitude * perturbationDirections[1]).normalized;
downrangeDelta = Vector3d.Dot(downrangeDirection, perturbationDirections[0]) * deltas[0]
+ Vector3d.Dot(downrangeDirection, perturbationDirections[1]) * deltas[1];
}
else
{
//If we aren't allowed to burn prograde, downrange component of the landing
//position has to be controlled by radial+/- burns:
downrangeDirection = perturbationDirections[1];
downrangeDelta = deltas[1];
}
//Now solve a 2x2 system of linear equations to determine the linear combination
//of perturbationDirection01 and normal+ that will give the desired offset in the
//predicted landing position.
Matrix2x2 A = new Matrix2x2(
downrangeDelta.sqrMagnitude, Vector3d.Dot(downrangeDelta, deltas[2]),
Vector3d.Dot(downrangeDelta, deltas[2]), deltas[2].sqrMagnitude
);
Vector2d b = new Vector2d(Vector3d.Dot(desiredDelta, downrangeDelta), Vector3d.Dot(desiredDelta, deltas[2]));
Vector2d coeffs = A.inverse() * b;
Vector3d courseCorrection = coeffs.x * downrangeDirection + coeffs.y * perturbationDirections[2];
return(courseCorrection);
}
// Estimate the delta-V of the correction burn that would be required to put us on
// course for the target
public Vector3d ComputeCourseCorrection(bool allowPrograde)
{
// actualLandingPosition is the predicted actual landing position
Vector3d actualLandingPosition = RotatedLandingSite - mainBody.position;
// orbitLandingPosition is the point where our current orbit intersects the planet
double endRadius = mainBody.Radius + DecelerationEndAltitude() - 100;
// Seems we are already landed ?
if (endRadius > orbit.ApR || vessel.LandedOrSplashed)
StopLanding();
Vector3d orbitLandingPosition;
if (orbit.PeR < endRadius)
orbitLandingPosition = orbit.SwappedRelativePositionAtUT(orbit.NextTimeOfRadius(vesselState.time, endRadius));
else
orbitLandingPosition = orbit.SwappedRelativePositionAtUT(orbit.NextPeriapsisTime(vesselState.time));
// convertOrbitToActual is a rotation that rotates orbitLandingPosition on actualLandingPosition
Quaternion convertOrbitToActual = Quaternion.FromToRotation(orbitLandingPosition, actualLandingPosition);
// Consider the effect small changes in the velocity in each of these three directions
Vector3d[] perturbationDirections = { vesselState.surfaceVelocity.normalized, vesselState.radialPlusSurface, vesselState.normalPlusSurface };
// Compute the effect burns in these directions would
// have on the landing position, where we approximate the landing position as the place
// the perturbed orbit would intersect the planet.
Vector3d[] deltas = new Vector3d[3];
for (int i = 0; i < 3; i++)
{
const double perturbationDeltaV = 1; //warning: hard experience shows that setting this too low leads to bewildering bugs due to finite precision of Orbit functions
Orbit perturbedOrbit = orbit.PerturbedOrbit(vesselState.time, perturbationDeltaV * perturbationDirections[i]); //compute the perturbed orbit
double perturbedLandingTime;
if (perturbedOrbit.PeR < endRadius) perturbedLandingTime = perturbedOrbit.NextTimeOfRadius(vesselState.time, endRadius);
else perturbedLandingTime = perturbedOrbit.NextPeriapsisTime(vesselState.time);
Vector3d perturbedLandingPosition = perturbedOrbit.SwappedRelativePositionAtUT(perturbedLandingTime); //find where it hits the planet
Vector3d landingDelta = perturbedLandingPosition - orbitLandingPosition; //find the difference between that and the original orbit's intersection point
landingDelta = convertOrbitToActual * landingDelta; //rotate that difference vector so that we can now think of it as starting at the actual landing position
landingDelta = Vector3d.Exclude(actualLandingPosition, landingDelta); //project the difference vector onto the plane tangent to the actual landing position
deltas[i] = landingDelta / perturbationDeltaV; //normalize by the delta-V considered, so that deltas now has units of meters per (meter/second) [i.e., seconds]
}
// Now deltas stores the predicted offsets in landing position produced by each of the three perturbations.
// We now figure out the offset we actually want
// First we compute the target landing position. We have to convert the latitude and longitude of the target
// into a position. We can't just get the current position of those coordinates, because the planet will
// rotate during the descent, so we have to account for that.
Vector3d desiredLandingPosition = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0);
float bodyRotationAngleDuringDescent = (float)(360 * (prediction.endUT - vesselState.time) / mainBody.rotationPeriod);
Quaternion bodyRotationDuringFall = Quaternion.AngleAxis(bodyRotationAngleDuringDescent, mainBody.angularVelocity.normalized);
desiredLandingPosition = bodyRotationDuringFall * desiredLandingPosition;
Vector3d desiredDelta = desiredLandingPosition - actualLandingPosition;
desiredDelta = Vector3d.Exclude(actualLandingPosition, desiredDelta);
// Now desiredDelta gives the desired change in our actual landing position (projected onto a plane
// tangent to the actual landing position).
Vector3d downrangeDirection;
Vector3d downrangeDelta;
if (allowPrograde)
{
// Construct the linear combination of the prograde and radial+ perturbations
// that produces the largest effect on the landing position. The Math.Sign is to
// detect and handle the case where radial+ burns actually bring the landing sign closer
// (e.g. when we are traveling close to straight up)
downrangeDirection = (deltas[0].magnitude * perturbationDirections[0]
+ Math.Sign(Vector3d.Dot(deltas[0], deltas[1])) * deltas[1].magnitude * perturbationDirections[1]).normalized;
downrangeDelta = Vector3d.Dot(downrangeDirection, perturbationDirections[0]) * deltas[0]
+ Vector3d.Dot(downrangeDirection, perturbationDirections[1]) * deltas[1];
}
else
{
// If we aren't allowed to burn prograde, downrange component of the landing
// position has to be controlled by radial+/- burns:
downrangeDirection = perturbationDirections[1];
downrangeDelta = deltas[1];
}
// Now solve a 2x2 system of linear equations to determine the linear combination
// of perturbationDirection01 and normal+ that will give the desired offset in the
// predicted landing position.
Matrix2x2 A = new Matrix2x2(
downrangeDelta.sqrMagnitude, Vector3d.Dot(downrangeDelta, deltas[2]),
Vector3d.Dot(downrangeDelta, deltas[2]), deltas[2].sqrMagnitude
);
Vector2d b = new Vector2d(Vector3d.Dot(desiredDelta, downrangeDelta), Vector3d.Dot(desiredDelta, deltas[2]));
Vector2d coeffs = A.inverse() * b;
Vector3d courseCorrection = coeffs.x * downrangeDirection + coeffs.y * perturbationDirections[2];
return courseCorrection;
}
///////////////////////////////////////
// REENTRY SIMULATION /////////////////
///////////////////////////////////////
//figure out what we wish our velocity were right now if we want to land at the user's target
//we do this by first doing a pair of hypothetical simulations, which we have extra velocity in
//two different orthogonal directions. By observing how this affects the landing site we compute the change in our velocity
//necessary to produce the desired change in our predicted landing location
void computeVelocityCorrection()
{
double offsetMagnitude = -5.0;
//simulate a trajectory where we have extra velocity in the A direction
LandingPrediction resultA = simulateReentryRK4(simulationTimeStep * 10.0, offsetMagnitude * velAUnit);
//if simulation doesn't hit the ground, we should burn retrograde until it does. set a goal that will cause us to deorbit
if (resultA.outcome != LandingPrediction.Outcome.LANDED)
{
if (landStep == LandStep.COURSE_CORRECTIONS && resultA.outcome == LandingPrediction.Outcome.NO_REENTRY)
{
gaveUpOnCourseCorrections = true;
turnOffSteering();
core.controlRelease(this);
print("A: gaveup, end condition = " + resultA.outcome);
}
velocityCorrection = -10 * vesselState.velocityVesselSurfaceUnit;
return;
}
//simulate a trajectory where we have extra velocity in the B direction
LandingPrediction resultB = simulateReentryRK4(simulationTimeStep * 10.0, offsetMagnitude * velBUnit);
if (resultB.outcome != LandingPrediction.Outcome.LANDED)
{
if (landStep == LandStep.COURSE_CORRECTIONS && resultB.outcome == LandingPrediction.Outcome.NO_REENTRY)
{
gaveUpOnCourseCorrections = true;
turnOffSteering();
core.controlRelease(this);
print("B: gaveup, end condition = " + resultB.outcome);
}
velocityCorrection = -10 * vesselState.velocityVesselSurfaceUnit;
return;
}
//simulate a trajectory with our current velocity
LandingPrediction resultCurrent = simulateReentryRK4(simulationTimeStep * 10.0, Vector3d.zero);
if (resultCurrent.outcome != LandingPrediction.Outcome.LANDED)
{
if (landStep == LandStep.COURSE_CORRECTIONS && resultCurrent.outcome == LandingPrediction.Outcome.NO_REENTRY)
{
gaveUpOnCourseCorrections = true;
turnOffSteering();
core.controlRelease(this);
print("C: gaveup, end condition = " + resultCurrent.outcome);
}
velocityCorrection = -10 * vesselState.velocityVesselSurfaceUnit;
return;
}
Vector2d sepA = latLonSeparation(resultCurrent.landingLatitude, resultCurrent.landingLongitude,
resultA.landingLatitude, resultA.landingLongitude) / offsetMagnitude;
Vector2d sepB = latLonSeparation(resultCurrent.landingLatitude, resultCurrent.landingLongitude,
resultB.landingLatitude, resultB.landingLongitude) / offsetMagnitude;
//construct a matrix that, when multiplied by a velocity offset vector2, gives the resulting offset in the landing position
Matrix2x2 partialsMatrix = new Matrix2x2(sepA.x, sepB.x, sepA.y, sepB.y);
//invert this to get a matrix that tells how to change our velocity to produce a
//give change in our landing site:
Matrix2x2 inversePartialsMatrix = partialsMatrix.inverse();
Vector2d sepDesired = latLonSeparation(prediction.landingLatitude, prediction.landingLongitude, targetLatitude, targetLongitude);
Vector2d correctionMagnitudes = inversePartialsMatrix.times(sepDesired);
velocityCorrection = correctionMagnitudes.x * velAUnit + correctionMagnitudes.y * velBUnit;
velocityCorrectionSet = true;
}
