// Get zenith angle of sun
public static float ZenithAngle(Vessel vessel, CelestialBody body)
{
// Oh I'm so clever. no need for EoT madness
return(Mathf.Deg2Rad * (float)Vector3d.Angle(body.position - vessel.GetWorldPos3D(), vessel.GetWorldPos3D() - FlightGlobals.Bodies[0].position));
}
/// <summary>
/// Automatically guides the ship to face a desired orientation
/// </summary>
/// <param name="target">The desired orientation</param>
/// <param name="c">The FlightCtrlState for the current vessel.</param>
/// <param name="fc">The flight computer carrying out the slew</param>
public static void SteerShipToward(Quaternion target, FlightCtrlState c, RemoteTech.FlightComputer fc)
{
// Add support for roll-less targets later -- Starstrider42
var fixedRoll = true;
var vessel = fc.Vessel;
var momentOfInertia = GetTrueMoI(vessel);
var vesselReference = vessel.GetTransform();
//---------------------------------------
// Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);
Vector3d torque = GetTorque(vessel, c.mainThrottle);
Vector3d inertia = GetEffectiveInertia(vessel, torque);
//err.Scale(SwapYZ(Vector3d.Scale(MoI, Inverse(torque))));
Vector3d normFactor = SwapYZ(Vector3d.Scale(momentOfInertia, Inverse(torque)));
// Find out the real shorter way to turn were we want to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0f;
var err = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
fixedRoll ?
((delta.eulerAngles.z > 180) ?
(delta.eulerAngles.z - 360.0F) :
delta.eulerAngles.z) * Math.PI / 180.0F
: 0F
);
err += SwapYZ(inertia) / 2;
err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(normFactor);
// angular velocity:
Vector3d omega = SwapYZ(vessel.angularVelocity);
omega.Scale(normFactor);
Vector3d pidAction = fc.pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1));
fc.lastAct = act;
// end MechJeb import
//---------------------------------------
float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f);
float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));
act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit);
act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit);
act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit);
c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit);
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit);
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit);
}
bool CheckPreconditions(Orbit o, double UT)
{
errorMessage = "";
bool error = false;
string burnAltitude = MuUtils.ToSI(o.Radius(UT) - o.referenceBody.Radius) + "m";
switch (operation)
{
case Operation.CIRCULARIZE:
break;
case Operation.ELLIPTICIZE:
if (o.referenceBody.Radius + newPeA > o.Radius(UT))
{
error = true;
errorMessage = "new periapsis cannot be higher than the altitude of the burn (" + burnAltitude + ")";
}
else if (o.referenceBody.Radius + newApA < o.Radius(UT))
{
error = true;
errorMessage = "new apoapsis cannot be lower than the altitude of the burn (" + burnAltitude + ")";
}
else if (newPeA < -o.referenceBody.Radius)
{
error = true;
errorMessage = "new periapsis cannot be lower than minus the radius of " + o.referenceBody.theName + "(-" + MuUtils.ToSI(o.referenceBody.Radius, 3) + "m)";
}
break;
case Operation.PERIAPSIS:
if (o.referenceBody.Radius + newPeA > o.Radius(UT))
{
error = true;
errorMessage = "new periapsis cannot be higher than the altitude of the burn (" + burnAltitude + ")";
}
else if (newPeA < -o.referenceBody.Radius)
{
error = true;
errorMessage = "new periapsis cannot be lower than minus the radius of " + o.referenceBody.theName + "(-" + MuUtils.ToSI(o.referenceBody.Radius, 3) + "m)";
}
break;
case Operation.APOAPSIS:
if (o.referenceBody.Radius + newApA < o.Radius(UT))
{
error = true;
errorMessage = "new apoapsis cannot be lower than the altitude of the burn (" + burnAltitude + ")";
}
break;
case Operation.INCLINATION:
break;
case Operation.PLANE:
if (!core.target.NormalTargetExists)
{
error = true;
errorMessage = "must select a target to match planes with.";
}
else if (o.referenceBody != core.target.Orbit.referenceBody)
{
error = true;
errorMessage = "can only match planes with an object in the same sphere of influence.";
}
else if (timeReference == TimeReference.REL_ASCENDING)
{
if (!o.AscendingNodeExists(core.target.Orbit))
{
error = true;
errorMessage = "ascending node with target doesn't exist.";
}
}
else
{
if (!o.DescendingNodeExists(core.target.Orbit))
{
error = true;
errorMessage = "descending node with target doesn't exist.";
}
}
break;
case Operation.TRANSFER:
if (!core.target.NormalTargetExists)
{
error = true;
errorMessage = "must select a target for the Hohmann transfer.";
}
else if (o.referenceBody != core.target.Orbit.referenceBody)
{
error = true;
errorMessage = "target for Hohmann transfer must be in the same sphere of influence.";
}
else if (o.eccentricity > 1)
{
error = true;
errorMessage = "starting orbit for Hohmann transfer must not be hyperbolic.";
}
else if (core.target.Orbit.eccentricity > 1)
{
error = true;
errorMessage = "target orbit for Hohmann transfer must not be hyperbolic.";
}
else if (o.RelativeInclination(core.target.Orbit) > 30 && o.RelativeInclination(core.target.Orbit) < 150)
{
errorMessage = "Warning: target's orbital plane is at a " + o.RelativeInclination(core.target.Orbit).ToString("F0") + "º angle to starting orbit's plane (recommend at most 30º). Planned transfer may not intercept target properly.";
}
else if (o.eccentricity > 0.2)
{
errorMessage = "Warning: Recommend starting Hohmann transfers from a near-circular orbit (eccentricity < 0.2). Planned transfer is starting from an orbit with eccentricity " + o.eccentricity.ToString("F2") + " and so may not intercept target properly.";
}
break;
case Operation.COURSE_CORRECTION:
if (!core.target.NormalTargetExists)
{
error = true;
errorMessage = "must select a target for the course correction.";
}
else if (o.referenceBody != core.target.Orbit.referenceBody)
{
error = true;
errorMessage = "target for course correction must be in the same sphere of influence";
}
else if (o.NextClosestApproachTime(core.target.Orbit, UT) < UT + 1 ||
o.NextClosestApproachDistance(core.target.Orbit, UT) > core.target.Orbit.semiMajorAxis * 0.2)
{
errorMessage = "Warning: orbit before course correction doesn't seem to approach target very closely. Planned course correction may be extreme. Recommend plotting an approximate intercept orbit and then plotting a course correction.";
}
break;
case Operation.INTERPLANETARY_TRANSFER:
if (!core.target.NormalTargetExists)
{
error = true;
errorMessage = "must select a target for the interplanetary transfer.";
}
else if (o.referenceBody.referenceBody == null)
{
error = true;
errorMessage = "doesn't make sense to plot an interplanetary transfer from an orbit around " + o.referenceBody.theName + ".";
}
else if (o.referenceBody.referenceBody != core.target.Orbit.referenceBody)
{
error = true;
if (o.referenceBody == core.target.Orbit.referenceBody)
{
errorMessage = "use regular Hohmann transfer function to intercept another body orbiting " + o.referenceBody.theName + ".";
}
else
{
errorMessage = "an interplanetary transfer from within " + o.referenceBody.theName + "'s sphere of influence must target a body that orbits " + o.referenceBody.theName + "'s parent, " + o.referenceBody.referenceBody.theName + ".";
}
}
else if (o.referenceBody.orbit.RelativeInclination(core.target.Orbit) > 30)
{
errorMessage = "Warning: target's orbital plane is at a " + o.RelativeInclination(core.target.Orbit).ToString("F0") + "º angle to " + o.referenceBody.theName + "'s orbital plane (recommend at most 30º). Planned interplanetary transfer may not intercept target properly.";
}
else
{
double relativeInclination = Vector3d.Angle(o.SwappedOrbitNormal(), o.referenceBody.orbit.SwappedOrbitNormal());
if (relativeInclination > 10)
{
errorMessage = "Warning: Recommend starting interplanetary transfers from " + o.referenceBody.theName + " from an orbit in the same plane as " + o.referenceBody.theName + "'s orbit around " + o.referenceBody.referenceBody.theName + ". Starting orbit around " + o.referenceBody.theName + " is inclined " + relativeInclination.ToString("F1") + "º with respect to " + o.referenceBody.theName + "'s orbit around " + o.referenceBody.referenceBody.theName + " (recommend < 10º). Planned transfer may not intercept target properly.";
}
else if (o.eccentricity > 0.2)
{
errorMessage = "Warning: Recommend starting interplanetary transfers from a near-circular orbit (eccentricity < 0.2). Planned transfer is starting from an orbit with eccentricity " + o.eccentricity.ToString("F2") + " and so may not intercept target properly.";
}
}
break;
case Operation.MOON_RETURN:
if (o.referenceBody.referenceBody == null)
{
error = true;
errorMessage = o.referenceBody.theName + " is not orbiting another body you could return to.";
}
else if (o.eccentricity > 0.2)
{
errorMessage = "Warning: Recommend starting moon returns from a near-circular orbit (eccentricity < 0.2). Planned return is starting from an orbit with eccentricity " + o.eccentricity.ToString("F2") + " and so may not be accurate.";
}
break;
case Operation.LAMBERT:
if (!core.target.NormalTargetExists)
{
error = true;
errorMessage = "must select a target to intercept.";
}
else if (o.referenceBody != core.target.Orbit.referenceBody)
{
error = true;
errorMessage = "target must be in the same sphere of influence.";
}
break;
case Operation.KILL_RELVEL:
if (!core.target.NormalTargetExists)
{
error = true;
errorMessage = "must select a target to match velocities with.";
}
else if (o.referenceBody != core.target.Orbit.referenceBody)
{
error = true;
errorMessage = "target must be in the same sphere of influence.";
}
break;
}
if (error)
{
errorMessage = "Couldn't plot maneuver: " + errorMessage;
}
return(!error);
}
private double NormaliseAngle(Vector3d vector1, Vector3d vector2)
{
vector1 = Vector3d.Project(new Vector3d(vector1.x, 0, vector1.z), vector1);
vector2 = Vector3d.Project(new Vector3d(vector2.x, 0, vector2.z), vector2);
return(Vector3d.Angle(vector1, vector2));
}
public override AutopilotStep Drive(FlightCtrlState s)
{
if (vessel.LandedOrSplashed)
{
core.landing.StopLanding();
return(null);
}
// TODO perhaps we should pop the parachutes at this point, or at least consider it depending on the altitude.
double minalt = Math.Min(vesselState.altitudeBottom, Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue));
if (vesselState.limitedMaxThrustAccel < vesselState.gravityForce.magnitude)
{
// if we have TWR < 1, just try as hard as we can to decelerate:
// (we need this special case because otherwise the calculations spit out NaN's)
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL;
core.thrust.trans_kill_h = true;
core.thrust.trans_spd_act = 0;
}
else if (minalt > 200)
{
if ((vesselState.surfaceVelocity.magnitude > 5) && (Vector3d.Angle(vesselState.surfaceVelocity, vesselState.up) < 80))
{
// if we have positive vertical velocity, point up and don't thrust:
core.attitude.attitudeTo(Vector3d.up, AttitudeReference.SURFACE_NORTH, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT;
core.thrust.trans_spd_act = 0;
}
else if ((vesselState.surfaceVelocity.magnitude > 5) && (Vector3d.Angle(vesselState.forward, -vesselState.surfaceVelocity) > 45))
{
// if we're not facing approximately retrograde, turn to point retrograde and don't thrust:
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT;
core.thrust.trans_spd_act = 0;
}
else
{
//if we're above 200m, point retrograde and control surface velocity:
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE;
//core.thrust.trans_spd_act = (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * minalt) * 0.90F;
Vector3d estimatedLandingPosition = vesselState.CoM + vesselState.surfaceVelocity.sqrMagnitude / (2 * vesselState.limitedMaxThrustAccel) * vesselState.surfaceVelocity.normalized;
double terrainRadius = mainBody.Radius + mainBody.TerrainAltitude(estimatedLandingPosition);
IDescentSpeedPolicy aggressivePolicy = new GravityTurnDescentSpeedPolicy(terrainRadius, mainBody.GeeASL * 9.81, vesselState.limitedMaxThrustAccel);
core.thrust.trans_spd_act = (float)(aggressivePolicy.MaxAllowedSpeed(vesselState.CoM - mainBody.position, vesselState.surfaceVelocity));
}
}
else
{
// last 200 meters:
core.thrust.trans_spd_act = -Mathf.Lerp(0, (float)Math.Sqrt((vesselState.limitedMaxThrustAccel - vesselState.localg) * 2 * 200) * 0.90F, (float)minalt / 200);
// take into account desired landing speed:
core.thrust.trans_spd_act = (float)Math.Min(-core.landing.touchdownSpeed, core.thrust.trans_spd_act);
// core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL;
// core.thrust.trans_kill_h = true;
// if (Math.Abs(Vector3d.Angle(-vessel.surfaceVelocity, vesselState.up)) < 10)
if (vesselState.speedSurfaceHorizontal < 5)
{
// if we're falling more or less straight down, control vertical speed and
// kill horizontal velocity
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL;
core.thrust.trans_kill_h = true;
}
else
{
// if we're falling at a significant angle from vertical, our vertical speed might be
// quite small but we might still need to decelerate. Control the total speed instead
// by thrusting directly retrograde
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE;
core.thrust.trans_spd_act *= -1;
}
}
status = "Final descent: " + vesselState.altitudeBottom.ToString("F0") + "m above terrain";
// ComputeCourseCorrection doesn't work close to the ground
/* if (core.landing.landAtTarget)
* {
* core.rcs.enabled = true;
* Vector3d deltaV = core.landing.ComputeCourseCorrection(false);
* core.rcs.SetWorldVelocityError(deltaV);
*
* status += "\nDV: " + deltaV.magnitude.ToString("F3") + " m/s";
* } */
return(this);
}
// provides AoA limiting and roll control
// provides no ground tracking and should only be called by autopilots like PVG that deeply know what they're doing with yaw control
// (possibly should be moved into the attitude controller, but right now it collaborates too heavily with the ascent autopilot)
//
protected void attitudeTo(double desiredPitch, double desiredHeading)
{
/*
* Vector6 rcs = vesselState.rcsThrustAvailable;
*
* // FIXME? should this be up/down and not forward/back? seems wrong? why was i using down before for the ullage direction?
* bool has_rcs = vessel.hasEnabledRCSModules() && vessel.ActionGroups[KSPActionGroup.RCS] && ( rcs.left > 0.01 ) && ( rcs.right > 0.01 ) && ( rcs.forward > 0.01 ) && ( rcs.back > 0.01 );
*
* if ( (vesselState.thrustCurrent / vesselState.thrustAvailable < 0.50) && !has_rcs )
* {
* // if engines are spooled up at less than 50% and we have no RCS in the stage, do not issue any guidance commands yet
* return;
* }
*/
Vector3d desiredHeadingVector = Math.Sin(desiredHeading * UtilMath.Deg2Rad) * vesselState.east + Math.Cos(desiredHeading * UtilMath.Deg2Rad) * vesselState.north;
Vector3d desiredThrustVector = Math.Cos(desiredPitch * UtilMath.Deg2Rad) * desiredHeadingVector
+ Math.Sin(desiredPitch * UtilMath.Deg2Rad) * vesselState.up;
desiredThrustVector = desiredThrustVector.normalized;
thrustVectorForNavball = desiredThrustVector;
/* old style AoA limiter */
if (autopilot.limitAoA && !autopilot.limitQaEnabled)
{
float fade = vesselState.dynamicPressure < autopilot.aoALimitFadeoutPressure ? (float)(autopilot.aoALimitFadeoutPressure / vesselState.dynamicPressure) : 1;
autopilot.currentMaxAoA = Math.Min(fade * autopilot.maxAoA, 180d);
autopilot.limitingAoA = vessel.altitude <mainBody.atmosphereDepth && Vector3.Angle(vesselState.surfaceVelocity, desiredThrustVector)> autopilot.currentMaxAoA;
if (autopilot.limitingAoA)
{
desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(autopilot.currentMaxAoA * Mathf.Deg2Rad), 1).normalized;
}
}
/* AoA limiter for PVG */
if (autopilot.limitQaEnabled)
{
double lim = MuUtils.Clamp(autopilot.limitQa, 100, 10000);
autopilot.limitingAoA = vesselState.dynamicPressure * Vector3.Angle(vesselState.surfaceVelocity, desiredThrustVector) * UtilMath.Deg2Rad > lim;
if (autopilot.limitingAoA)
{
autopilot.currentMaxAoA = lim / vesselState.dynamicPressure * UtilMath.Rad2Deg;
desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(autopilot.currentMaxAoA * UtilMath.Deg2Rad), 1).normalized;
}
}
double pitch = 90 - Vector3d.Angle(desiredThrustVector, vesselState.up);
double hdg;
if (pitch > 89.9)
{
hdg = desiredHeading;
}
else
{
hdg = MuUtils.ClampDegrees360(UtilMath.Rad2Deg * Math.Atan2(Vector3d.Dot(desiredThrustVector, vesselState.east), Vector3d.Dot(desiredThrustVector, vesselState.north)));
}
if (autopilot.forceRoll)
{
core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && (vesselState.altitudeBottom > 50));
if (desiredPitch == 90.0)
{
core.attitude.attitudeTo(hdg, pitch, autopilot.verticalRoll, this, !vessel.Landed, !vessel.Landed, !vessel.Landed && (vesselState.altitudeBottom > 50));
}
else
{
core.attitude.attitudeTo(hdg, pitch, autopilot.turnRoll, this, !vessel.Landed, !vessel.Landed, !vessel.Landed && (vesselState.altitudeBottom > 50));
}
}
else
{
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
}
}
void CalculateSlope(double[] normalMapValues, PQS pqs, int?firstY, int?lastY, ref Texture2D normalMap, ref Texture2D slopeMap)
{
double dS = pqs.radius * 2 * Math.PI / width;
for (int y = 0; y < tile; y++)
{
for (var x = 0; x < tile; x++)
{
// force slope = 0 at the poles
if (y == firstY || y == lastY)
{
if (exportNormalMap || exportSatelliteMap)
{
if (y == firstY)
{
normalMap.SetPixel(x, y, new Color(1, 0, 0, 1));
}
if (y == lastY)
{
normalMap.SetPixel(x, y, new Color(0, 1, 0, 1));
}
}
if (exportSlopeMap)
{
slopeMap.SetPixel(x, y, slopeMin);
}
}
// otherwise calculate it from the terrain
else
{
int xN = x - 1;
int xP = x + 1;
int yN = y - 1;
int yP = y + 1;
// shift all by one since `normalMapValues` has an extra frame of 1 pixel
double dX = normalMapValues[((y + 1) * (tile + 2)) + (xP + 1)] - normalMapValues[((y + 1) * (tile + 2)) + (xN + 1)];
double dY = normalMapValues[((yP + 1) * (tile + 2)) + (x + 1)] - normalMapValues[((yN + 1) * (tile + 2)) + (x + 1)];
if (exportNormalMap || exportSatelliteMap)
{
double slopeX = (1 + dX / Math.Pow(dX * dX + dS * dS, 0.5) * normalStrength) / 2;
double slopeY = (1 - dY / Math.Pow(dY * dY + dS * dS, 0.5) * normalStrength) / 2;
normalMap.SetPixel(x, y, new Color((float)slopeY, (float)slopeY, (float)slopeY, (float)slopeX));
}
if (exportSlopeMap)
{
Vector3d vX = new Vector3d(dS, 0, dX);
Vector3d vY = new Vector3d(0, dS, dY);
Vector3d n = Vector3d.Cross(vX, vY);
double slope = Vector3d.Angle(new Vector3d(0, 0, 1), n) / 90d;
if (slope > 1)
{
slope = 2 - slope;
}
slopeMap.SetPixel(x, y, Color.Lerp(slopeMin, slopeMax, (float)slope));
}
}
}
}
}
//Computes the time and delta-V of an ejection burn to a Hohmann transfer from one planet to another.
//It's assumed that the initial orbit around the first planet is circular, and that this orbit
//is in the same plane as the orbit of the first planet around the sun. It's also assumed that
//the target planet has a fairly low relative inclination with respect to the first planet. If the
//inclination change is nonzero you should also do a mid-course correction burn, as computed by
//DeltaVForCourseCorrection.
public static Vector3d DeltaVAndTimeForInterplanetaryTransferEjection(Orbit o, double UT, Orbit target, bool syncPhaseAngle, out double burnUT)
{
Orbit planetOrbit = o.referenceBody.orbit;
//Compute the time and dV for a Hohmann transfer where we pretend that we are the planet we are orbiting.
//This gives us the "ideal" deltaV and UT of the ejection burn, if we didn't have to worry about waiting for the right
//ejection angle and if we didn't have to worry about the planet's gravity dragging us back and increasing the required dV.
double idealBurnUT;
Vector3d idealDeltaV;
if (syncPhaseAngle)
{
//time the ejection burn to intercept the target
idealDeltaV = DeltaVAndTimeForHohmannTransfer(planetOrbit, target, UT, out idealBurnUT);
}
else
{
//don't time the ejection burn to intercept the target; we just care about the final peri/apoapsis
idealBurnUT = UT;
if (target.semiMajorAxis < planetOrbit.semiMajorAxis)
{
idealDeltaV = DeltaVToChangePeriapsis(planetOrbit, idealBurnUT, target.semiMajorAxis);
}
else
{
idealDeltaV = DeltaVToChangeApoapsis(planetOrbit, idealBurnUT, target.semiMajorAxis);
}
}
//Compute the actual transfer orbit this ideal burn would lead to.
Orbit transferOrbit = planetOrbit.PerturbedOrbit(idealBurnUT, idealDeltaV);
//Now figure out how to approximately eject from our current orbit into the Hohmann orbit we just computed.
//Assume we want to exit the SOI with the same velocity as the ideal transfer orbit at idealUT -- i.e., immediately
//after the "ideal" burn we used to compute the transfer orbit. This isn't quite right.
//We intend to eject from our planet at idealUT and only several hours later will we exit the SOI. Meanwhile
//the transfer orbit will have acquired a slightly different velocity, which we should correct for. Maybe
//just add in (1/2)(sun gravity)*(time to exit soi)^2 ? But how to compute time to exit soi? Or maybe once we
//have the ejection orbit we should just move the ejection burn back by the time to exit the soi?
Vector3d soiExitVelocity = idealDeltaV;
//project the desired exit direction into the current orbit plane to get the feasible exit direction
Vector3d inPlaneSoiExitDirection = Vector3d.Exclude(o.SwappedOrbitNormal(), soiExitVelocity).normalized;
//compute the angle by which the trajectory turns between periapsis (where we do the ejection burn)
//and SOI exit (approximated as radius = infinity)
double soiExitEnergy = 0.5 * soiExitVelocity.sqrMagnitude - o.referenceBody.gravParameter / o.referenceBody.sphereOfInfluence;
double ejectionRadius = o.semiMajorAxis; //a guess, good for nearly circular orbits
double ejectionKineticEnergy = soiExitEnergy + o.referenceBody.gravParameter / ejectionRadius;
double ejectionSpeed = Math.Sqrt(2 * ejectionKineticEnergy);
//construct a sample ejection orbit
Vector3d ejectionOrbitInitialVelocity = ejectionSpeed * (Vector3d)o.referenceBody.transform.right;
Vector3d ejectionOrbitInitialPosition = o.referenceBody.position + ejectionRadius * (Vector3d)o.referenceBody.transform.up;
Orbit sampleEjectionOrbit = MuUtils.OrbitFromStateVectors(ejectionOrbitInitialPosition, ejectionOrbitInitialVelocity, o.referenceBody, 0);
double ejectionOrbitDuration = sampleEjectionOrbit.NextTimeOfRadius(0, o.referenceBody.sphereOfInfluence);
Vector3d ejectionOrbitFinalVelocity = sampleEjectionOrbit.SwappedOrbitalVelocityAtUT(ejectionOrbitDuration);
double turningAngle = Math.Abs(Vector3d.Angle(ejectionOrbitInitialVelocity, ejectionOrbitFinalVelocity));
//rotate the exit direction by 90 + the turning angle to get a vector pointing to the spot in our orbit
//where we should do the ejection burn. Then convert this to a true anomaly and compute the time closest
//to planetUT at which we will pass through that true anomaly.
Vector3d ejectionPointDirection = Quaternion.AngleAxis(-(float)(90 + turningAngle), o.SwappedOrbitNormal()) * inPlaneSoiExitDirection;
double ejectionTrueAnomaly = o.TrueAnomalyFromVector(ejectionPointDirection);
burnUT = o.TimeOfTrueAnomaly(ejectionTrueAnomaly, idealBurnUT - o.period);
if ((idealBurnUT - burnUT > o.period / 2) || (burnUT < UT))
{
burnUT += o.period;
}
//rotate the exit direction by the turning angle to get a vector pointing to the spot in our orbit
//where we should do the ejection burn
Vector3d ejectionBurnDirection = Quaternion.AngleAxis(-(float)(turningAngle), o.SwappedOrbitNormal()) * inPlaneSoiExitDirection;
Vector3d ejectionVelocity = ejectionSpeed * ejectionBurnDirection;
Vector3d preEjectionVelocity = o.SwappedOrbitalVelocityAtUT(burnUT);
return(ejectionVelocity - preEjectionVelocity);
}
void FixedUpdateDeorbitBurn()
{
//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.RealMaxAtmosphereAltitude())
{
landStep = LandStep.COURSE_CORRECTIONS;
core.thrust.targetThrottle = 0;
return;
}
//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
Vector3d horizontalDV = OrbitalManeuverCalculator.DeltaVToChangePeriapsis(orbit, vesselState.time, 0.9 * mainBody.Radius); //Imagine we are going to deorbit now. Find the burn that would lower our periapsis to -10% of the planet's radius
Orbit forwardDeorbitTrajectory = orbit.PerturbedOrbit(vesselState.time, horizontalDV); //Compute the orbit that would put us on
double freefallTime = forwardDeorbitTrajectory.NextTimeOfRadius(vesselState.time, mainBody.Radius) - vesselState.time; //Find how long that orbit would take to impact the ground
double planetRotationDuringFreefall = 360 * freefallTime / mainBody.rotationPeriod; //Find how many degrees the planet will rotate during that time
Vector3d currentTargetRadialVector = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0); //Find the current vector from the planet center to the target landing site
Quaternion freefallPlanetRotation = Quaternion.AngleAxis((float)planetRotationDuringFreefall, mainBody.angularVelocity); //Construct a quaternion representing the rotation of the planet found above
Vector3d freefallEndTargetRadialVector = freefallPlanetRotation * currentTargetRadialVector; //Use this quaternion to find what the vector from the planet center to the target will be when we hit the ground
Vector3d freefallEndTargetPosition = mainBody.position + freefallEndTargetRadialVector; //Then find the actual position of the target at that time
Vector3d freefallEndHorizontalToTarget = Vector3d.Exclude(vesselState.up, freefallEndTargetPosition - vesselState.CoM).normalized; //Find a horizontal unit vector that points toward where the target will be when we hit the ground
Vector3d currentHorizontalVelocity = Vector3d.Exclude(vesselState.up, vesselState.velocityVesselOrbit); //Find our current horizontal velocity
double finalHorizontalSpeed = (currentHorizontalVelocity + horizontalDV).magnitude; //Find the desired horizontal speed after the deorbit burn
Vector3d finalHorizontalVelocity = finalHorizontalSpeed * freefallEndHorizontalToTarget; //Combine the desired speed and direction to get the desired velocity after the deorbi burn
//Compute the angle between the location of the target at the end of freefall and the normal to our orbit:
Vector3d currentRadialVector = vesselState.CoM - part.vessel.mainBody.position;
double targetAngleToOrbitNormal = Vector3d.Angle(orbit.SwappedOrbitNormal(), freefallEndTargetRadialVector);
targetAngleToOrbitNormal = Math.Min(targetAngleToOrbitNormal, 180 - targetAngleToOrbitNormal);
double targetAheadAngle = Vector3d.Angle(currentRadialVector, freefallEndTargetRadialVector); //How far ahead the target is, in degrees
double planeChangeAngle = Vector3d.Angle(currentHorizontalVelocity, freefallEndHorizontalToTarget); //The plane change required to get onto the deorbit trajectory, in degrees
//If the target is basically almost normal to our orbit, it doesn't matter when we deorbit; might as well do it now
//Otherwise, wait until the target is ahead
if (targetAngleToOrbitNormal < 10 ||
(targetAheadAngle < 90 && targetAheadAngle > 60 && planeChangeAngle < 90))
{
deorbitBurnTriggered = true;
}
if (deorbitBurnTriggered)
{
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp(true); //get out of warp
}
Vector3d deltaV = finalHorizontalVelocity - currentHorizontalVelocity;
core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, this);
if (deltaV.magnitude < 2.0)
{
landStep = LandStep.COURSE_CORRECTIONS;
}
status = "Doing high deorbit burn";
}
else
{
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT, this);
if (core.node.autowarp)
{
core.warp.WarpRegularAtRate((float)(orbit.period / 10));
}
status = "Moving to high deorbit burn point";
}
}
public void AimInDirection(Vector3 targetDirection, bool pitch = true, bool yaw = true)
{
if (!yawTransform)
{
return;
}
float deltaTime = Time.fixedDeltaTime;
Vector3 yawNormal = yawTransform.up;
Vector3 yawComponent = Vector3.ProjectOnPlane(targetDirection, yawNormal);
Vector3 pitchNormal = Vector3.Cross(yawComponent, yawNormal);
Vector3 pitchComponent = Vector3.ProjectOnPlane(targetDirection, pitchNormal);
float currentYaw = yawTransform.localEulerAngles.y;
float yawError = VectorUtils.SignedAngleDP(
Vector3.ProjectOnPlane(referenceTransform.forward, yawNormal),
yawComponent,
Vector3.Cross(yawNormal, referenceTransform.forward));
float yawOffset = Mathf.Abs(yawError);
float targetYawAngle = Mathf.Clamp((currentYaw + yawError + 180f) % 360f - 180f, -yawRange / 2, yawRange / 2); // clamped target yaw
float pitchError = (float)Vector3d.Angle(pitchComponent, yawNormal) - (float)Vector3d.Angle(referenceTransform.forward, yawNormal);
float currentPitch = -((pitchTransform.localEulerAngles.x + 180f) % 360f - 180f); // from current rotation transform
float targetPitchAngle = currentPitch - pitchError;
float pitchOffset = Mathf.Abs(targetPitchAngle - currentPitch);
targetPitchAngle = Mathf.Clamp(targetPitchAngle, minPitch, maxPitch); // clamp pitch
float linPitchMult = yawOffset > 0 ? Mathf.Clamp01((pitchOffset / yawOffset) * (yawSpeedDPS / pitchSpeedDPS)) : 1;
float linYawMult = pitchOffset > 0 ? Mathf.Clamp01((yawOffset / pitchOffset) * (pitchSpeedDPS / yawSpeedDPS)) : 1;
float yawSpeed;
float pitchSpeed;
if (smoothRotation)
{
yawSpeed = Mathf.Clamp(yawOffset * smoothMultiplier, 1f, yawSpeedDPS) * deltaTime;
pitchSpeed = Mathf.Clamp(pitchOffset * smoothMultiplier, 1f, pitchSpeedDPS) * deltaTime;
}
else
{
yawSpeed = yawSpeedDPS * deltaTime;
pitchSpeed = pitchSpeedDPS * deltaTime;
}
yawSpeed *= linYawMult;
pitchSpeed *= linPitchMult;
if (yawRange < 360 && Mathf.Abs(targetYawAngle) > 90 && Mathf.Sign(currentYaw) != Mathf.Sign(targetYawAngle))
{
targetYawAngle = 5 * Mathf.Sign(targetYawAngle);
}
if (yaw)
{
yawTransform.localRotation = Quaternion.RotateTowards(yawTransform.localRotation,
Quaternion.Euler(0, targetYawAngle, 0), yawSpeed);
}
if (pitch)
{
pitchTransform.localRotation = Quaternion.RotateTowards(pitchTransform.localRotation,
Quaternion.Euler(-targetPitchAngle, 0, 0), pitchSpeed);
}
}
/// <summary>
/// Update rover.
/// </summary>
/// <param name="currentTime">Current time.</param>
public void Update(double currentTime)
{
if (vessel.isActiveVessel)
{
status = "current";
return;
}
if (!bvActive || vessel.loaded)
{
status = "idle";
return;
}
Vector3d vesselPos = vessel.mainBody.position - vessel.GetWorldPos3D();
Vector3d toKerbol = vessel.mainBody.position - FlightGlobals.Bodies[0].position;
double angle = Vector3d.Angle(vesselPos, toKerbol);
// Speed penalties at twighlight and at night
if (angle > 90 && isManned)
{
speedMultiplier = 0.25;
}
else if (angle > 85 && isManned)
{
speedMultiplier = 0.5;
}
else if (angle > 80 && isManned)
{
speedMultiplier = 0.75;
}
else
{
speedMultiplier = 1.0;
}
// No moving at night, or when there's not enougth solar light for solar powered rovers
if (angle > 90 && solarPowered)
{
status = "awaiting sunlight";
lastTime = currentTime;
BVModule.SetValue("lastTime", currentTime.ToString());
vessel.protoVessel = new ProtoVessel(vesselConfigNode, HighLogic.CurrentGame);
return;
}
double deltaT = currentTime - lastTime;
double deltaS = AverageSpeed * deltaT;
double bearing = GeoUtils.InitialBearing(
vessel.latitude,
vessel.longitude,
targetLatitude,
targetLongitude
);
distanceTravelled += deltaS;
if (distanceTravelled >= distanceToTarget)
{
// vessel.latitude = targetLatitude;
// vessel.longitude = targetLongitude;
if (!MoveSafe(targetLatitude, targetLongitude))
{
distanceTravelled -= deltaS;
}
else
{
distanceTravelled = distanceToTarget;
bvActive = false;
BVModule.SetValue("isActive", "False");
BVModule.SetValue("distanceTravelled", distanceToTarget.ToString());
BVModule.SetValue("pathEncoded", "");
// BVModule.GetNode ("EVENTS").GetNode ("Activate").SetValue ("active", "True");
// BVModule.GetNode ("EVENTS").GetNode ("Deactivate").SetValue ("active", "False");
if (BonVoyage.Instance.AutoDewarp)
{
if (TimeWarp.CurrentRate > 3)
{
TimeWarp.SetRate(3, true);
}
if (TimeWarp.CurrentRate > 0)
{
TimeWarp.SetRate(0, false);
}
ScreenMessages.PostScreenMessage(vessel.vesselName + " has arrived to destination at " + vessel.mainBody.name);
}
HoneyImHome();
}
status = "idle";
}
else
{
int step = Convert.ToInt32(Math.Floor(distanceTravelled / PathFinder.StepSize));
double remainder = distanceTravelled % PathFinder.StepSize;
if (step < path.Count - 1)
{
bearing = GeoUtils.InitialBearing(
path[step].latitude,
path[step].longitude,
path[step + 1].latitude,
path[step + 1].longitude
);
}
else
{
bearing = GeoUtils.InitialBearing(
path[step].latitude,
path[step].longitude,
targetLatitude,
targetLongitude
);
}
double[] newCoordinates = GeoUtils.GetLatitudeLongitude(
path[step].latitude,
path[step].longitude,
bearing,
remainder,
vessel.mainBody.Radius
);
// vessel.latitude = newCoordinates[0];
// vessel.longitude = newCoordinates[1];
if (!MoveSafe(newCoordinates [0], newCoordinates [1]))
{
distanceTravelled -= deltaS;
status = "idle";
}
else
{
status = "roving";
}
}
// vessel.altitude = GeoUtils.TerrainHeightAt(vessel.latitude, vessel.longitude, vessel.mainBody);
Save(currentTime);
}
/// <summary>
/// Automatically guides the ship to face a desired orientation
/// </summary>
/// <param name="target">The desired orientation</param>
/// <param name="c">The FlightCtrlState for the current vessel.</param>
/// <param name="p">The Processor carrying out the slew</param>
/// <param name="ignoreRoll">[optional] to ignore the roll</param>
public static void SteerShipToward(Quaternion target, FlightCtrlState c, Processor p, bool ignoreRoll)
{
Vessel vessel = p.parentVessel;
// Add support for roll-less targets later -- Starstrider42
bool fixedRoll = !ignoreRoll;
Vector3d momentOfInertia = GetTrueMoI(vessel);
Transform vesselReference = vessel.GetTransform();
//---------------------------------------
// Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);
Vector3d torque = GetTorque(vessel, c.mainThrottle);
Vector3d spinMargin = GetStoppingAngle(vessel, torque);
// Allow for zero torque around some but not all axes
Vector3d normFactor;
normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0);
normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0);
normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0);
normFactor = SwapYZ(normFactor);
// Find out the real shorter way to turn were we want to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0f;
var err = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
fixedRoll ?
((delta.eulerAngles.z > 180) ?
(delta.eulerAngles.z - 360.0F) :
delta.eulerAngles.z) * Math.PI / 180.0F
: 0F
);
err += SwapYZ(spinMargin);
err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(normFactor);
// angular velocity:
Vector3d omega = SwapYZ(vessel.angularVelocity);
omega.Scale(normFactor);
Vector3d pidAction = p.pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = p.lastAct + (pidAction - p.lastAct) * (1 / ((p.Tf / TimeWarp.fixedDeltaTime) + 1));
p.lastAct = act;
// end MechJeb import
//---------------------------------------
float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f);
float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));
act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit);
act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit);
act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit);
c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit);
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit);
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit);
}
public override void Drive(FlightCtrlState s)
{
if (useSAS)
{
// TODO : This most likely require some love to use all the new SAS magic
_requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
if (!part.vessel.ActionGroups[KSPActionGroup.SAS])
{
part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
part.vessel.Autopilot.SAS.SetTargetOrientation(_requestedAttitude * Vector3.up, false);
lastSAS = _requestedAttitude;
}
else if (Quaternion.Angle(lastSAS, _requestedAttitude) > 10)
{
part.vessel.Autopilot.SAS.SetTargetOrientation(_requestedAttitude * Vector3.up, false);
lastSAS = _requestedAttitude;
}
else
{
part.vessel.Autopilot.SAS.SetTargetOrientation(_requestedAttitude * Vector3.up, true);
}
core.thrust.differentialThrottleDemandedTorque = Vector3d.zero;
}
else
{
// Direction we want to be facing
_requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
Transform vesselTransform = vessel.ReferenceTransform;
//Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselTransform.rotation) * _requestedAttitude);
// Find out the real shorter way to turn where we wan to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0;
// And the lowest roll
// Thanks to Crzyrndm
Vector3 normVec = Vector3.Cross(_requestedAttitude * Vector3.forward, vesselTransform.up);
Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * _requestedAttitude;
float rollError = Vector3.Angle(vesselTransform.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselTransform.forward));
// From here everything should use MOI order for Vectors (pitch, roll, yaw)
error = new Vector3d(
-rotDirection.y * Math.PI,
rollError * Mathf.Deg2Rad,
rotDirection.x * Math.PI
);
error.Scale(_axisControl);
Vector3d err = error + inertia * 0.5;
err = new Vector3d(
Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
// ( MoI / available torque ) factor:
Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.InvertNoNaN());
err.Scale(NormFactor);
// angular velocity:
Vector3d omega = vessel.angularVelocity;
//omega.x = vessel.angularVelocity.x;
//omega.y = vessel.angularVelocity.z; // y <=> z
//omega.z = vessel.angularVelocity.y; // z <=> y
omega.Scale(NormFactor);
if (Tf_autoTune)
{
tuneTf(torque);
}
setPIDParameters();
// angular velocity limit:
var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit),
Math.Sqrt(NormFactor.y * Math.PI * kWlimit),
Math.Sqrt(NormFactor.z * Math.PI * kWlimit));
pidAction = pid.Compute(err, omega, Wlimit);
// deadband
pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0;
pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0;
pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0;
// low pass filter, wf = 1/Tf:
Vector3d act = lastAct;
if (lowPassFilter)
{
act.x += (pidAction.x - lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0));
act.y += (pidAction.y - lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0));
act.z += (pidAction.z - lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0));
}
else
{
act = pidAction;
}
lastAct = act;
Vector3d deltaEuler = error * UtilMath.Rad2Deg;
SetFlightCtrlState(act, deltaEuler, s, 1);
act = new Vector3d(s.pitch, s.roll, s.yaw);
// Feed the control torque to the differential throttle
if (core.thrust.differentialThrottleSuccess)
{
core.thrust.differentialThrottleDemandedTorque = -Vector3d.Scale(act, vesselState.torqueDiffThrottle * vessel.ctrlState.mainThrottle);
}
}
}
public void Update()
{
if (lastUpdated.AddSeconds(1) > DateTime.Now)
{
return;
}
lastUpdated = DateTime.Now;
double currentTime = Planetarium.GetUniversalTime();
foreach (var rover in activeRovers)
{
if (rover.vessel.isActiveVessel)
{
rover.status = "current";
continue;
}
if (!rover.bvActive || rover.vessel.loaded)
{
rover.status = "idle";
continue;
}
Vector3d vesselPos = rover.vessel.mainBody.position - rover.vessel.GetWorldPos3D();
Vector3d toKerbol = rover.vessel.mainBody.position - FlightGlobals.Bodies[0].position;
double angle = Vector3d.Angle(vesselPos, toKerbol);
// No moving at night, or when there's not enougth solar light
if (angle >= 85 && rover.solarPowered)
{
rover.status = "awaiting sunlight";
rover.lastTime = currentTime;
rover.BVModule.SetValue("lastTime", currentTime.ToString());
rover.vessel.protoVessel = new ProtoVessel(rover.vesselConfigNode, HighLogic.CurrentGame);
continue;
}
double deltaT = currentTime - rover.lastTime;
double deltaS = rover.averageSpeed * deltaT;
double bearing = GeoUtils.InitialBearing(
rover.vessel.latitude,
rover.vessel.longitude,
rover.targetLatitude,
rover.targetLongitude
);
rover.distanceTravelled += deltaS;
if (rover.distanceTravelled >= rover.distanceToTarget)
{
rover.distanceTravelled = rover.distanceToTarget;
rover.vessel.latitude = rover.targetLatitude;
rover.vessel.longitude = rover.targetLongitude;
rover.bvActive = false;
rover.BVModule.SetValue("isActive", "False");
rover.BVModule.SetValue("distanceTravelled", rover.distanceToTarget.ToString());
rover.BVModule.GetNode("EVENTS").GetNode("Activate").SetValue("active", "True");
rover.BVModule.GetNode("EVENTS").GetNode("Deactivate").SetValue("active", "False");
if (autoDewarp)
{
TimeWarp.SetRate(0, false);
ScreenMessages.PostScreenMessage(rover.vessel.vesselName + " has arrived to destination at " + rover.vessel.mainBody.name);
}
MessageSystem.Message message = new MessageSystem.Message(
"Rover arrived",
//------------------------------------------
rover.vessel.vesselName + " has arrived to destination\nLAT:" +
rover.targetLatitude.ToString("F2") + "\nLON:" + rover.targetLongitude.ToString("F2") +
"\nAt " + rover.vessel.mainBody.name + ". \n" +
"Distance travelled: " + rover.distanceTravelled.ToString("N") + " meters",
//------------------------------------------
MessageSystemButton.MessageButtonColor.GREEN,
MessageSystemButton.ButtonIcons.COMPLETE
);
MessageSystem.Instance.AddMessage(message);
rover.status = "idle";
}
else
{
int step = Convert.ToInt32(Math.Floor(rover.distanceTravelled / 1000));
double remainder = rover.distanceTravelled % 1000;
if (step < rover.path.Count - 1)
{
bearing = GeoUtils.InitialBearing(
rover.path[step].latitude,
rover.path[step].longitude,
rover.path[step + 1].latitude,
rover.path[step + 1].longitude
);
}
else
{
bearing = GeoUtils.InitialBearing(
rover.path[step].latitude,
rover.path[step].longitude,
rover.targetLatitude,
rover.targetLongitude
);
}
double[] newCoordinates = GeoUtils.GetLatitudeLongitude(
rover.path[step].latitude,
rover.path[step].longitude,
bearing,
remainder,
rover.vessel.mainBody.Radius
);
rover.vessel.latitude = newCoordinates[0];
rover.vessel.longitude = newCoordinates[1];
rover.status = "roving";
}
rover.vessel.altitude = GeoUtils.TerrainHeightAt(rover.vessel.latitude, rover.vessel.longitude, rover.vessel.mainBody);
// rover.toTravel = rover.distanceToTarget - rover.distanceTravelled;
rover.lastTime = currentTime;
// Save data to protovessel
rover.vesselConfigNode.SetValue("lat", rover.vessel.latitude.ToString());
rover.vesselConfigNode.SetValue("lon", rover.vessel.longitude.ToString());
rover.vesselConfigNode.SetValue("alt", rover.vessel.altitude.ToString());
rover.vesselConfigNode.SetValue("landedAt", rover.vessel.mainBody.theName);
rover.BVModule.SetValue("distanceTravelled", (rover.distanceTravelled).ToString());
rover.BVModule.SetValue("lastTime", currentTime.ToString());
rover.vessel.protoVessel = new ProtoVessel(rover.vesselConfigNode, HighLogic.CurrentGame);
}
}
public static double GetRelativeInclination(this Orbit orbit, Orbit target)
{
return(Vector3d.Angle(orbit.GetOrbitNormal(), target.GetOrbitNormal()));
}
public void DriveUntargetedLanding(FlightCtrlState s)
{
if (vessel.LandedOrSplashed)
{
core.thrust.ThrustOff();
core.thrust.users.Remove(this);
StopLanding();
return;
}
double minalt = Math.Min(vesselState.altitudeBottom, Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue));
if (!deployedGears && (minalt < 1000))
{
DeployLandingGears();
}
if (vesselState.limitedMaxThrustAccel < vesselState.gravityForce.magnitude)
{
//if we have TWR < 1, just try as hard as we can to decelerate:
//(we need this special case because otherwise the calculations spit out NaN's)
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL;
core.thrust.trans_kill_h = true;
core.thrust.trans_spd_act = 0;
}
else if (minalt > 200)
{
if ((vesselState.velocityVesselSurface.magnitude > 5) && (Vector3d.Angle(vesselState.velocityVesselSurface, vesselState.up) < 80))
{
//if we have positive vertical velocity, point up and don't thrust:
core.attitude.attitudeTo(Vector3d.up, AttitudeReference.SURFACE_NORTH, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT;
core.thrust.trans_spd_act = 0;
}
else if ((vesselState.velocityVesselSurface.magnitude > 5) && (Vector3d.Angle(vesselState.forward, -vesselState.velocityVesselSurface) > 45))
{
//if we're not facing approximately retrograde, turn to point retrograde and don't thrust:
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT;
core.thrust.trans_spd_act = 0;
}
else
{
//if we're above 200m, point retrograde and control surface velocity:
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE;
//core.thrust.trans_spd_act = (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * minalt) * 0.90F;
Vector3d estimatedLandingPosition = vesselState.CoM + vesselState.velocityVesselSurface.sqrMagnitude / (2 * vesselState.limitedMaxThrustAccel) * vesselState.velocityVesselSurfaceUnit;
double terrainRadius = mainBody.Radius + mainBody.TerrainAltitude(estimatedLandingPosition);
IDescentSpeedPolicy aggressivePolicy = new GravityTurnDescentSpeedPolicy(terrainRadius, mainBody.GeeASL * 9.81, vesselState.limitedMaxThrustAccel);
core.thrust.trans_spd_act = (float)(aggressivePolicy.MaxAllowedSpeed(vesselState.CoM - mainBody.position, vesselState.velocityVesselSurface));
}
}
else
{
//last 200 meters:
core.thrust.trans_spd_act = -Mathf.Lerp(0, (float)Math.Sqrt((vesselState.limitedMaxThrustAccel - vesselState.gravityForce.magnitude) * 2 * 200) * 0.90F, (float)minalt / 200);
//take into account desired landing speed:
core.thrust.trans_spd_act = (float)Math.Min(-touchdownSpeed, core.thrust.trans_spd_act);
if (Math.Abs(Vector3d.Angle(-vesselState.velocityVesselSurface, vesselState.up)) < 10)
{
//if we're falling more or less straight down, control vertical speed and
//kill horizontal velocity
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL;
core.thrust.trans_kill_h = true;
}
else
{
//if we're falling at a significant angle from vertical, our vertical speed might be
//quite small but we might still need to decelerate. Control the total speed instead
//by thrusting directly retrograde
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE;
core.thrust.trans_spd_act *= -1;
}
}
status = "Final descent: " + vesselState.altitudeBottom.ToString("F0") + "m above terrain";
}
protected double srfvelPitch()
{
return(90.0 - Vector3d.Angle(vesselState.surfaceVelocity, vesselState.up));
}
public override void Drive(FlightCtrlState s)
{
if (useSAS)
{
Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
if (!part.vessel.ActionGroups[KSPActionGroup.SAS])
{
part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
part.vessel.Autopilot.SAS.LockHeading(target);
lastSAS = target;
}
else if (Quaternion.Angle(lastSAS, target) > 10)
{
part.vessel.Autopilot.SAS.LockHeading(target);
lastSAS = target;
}
else
{
part.vessel.Autopilot.SAS.LockHeading(target, true);
}
}
else
{
// Direction we want to be facing
Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * target);
Vector3d deltaEuler = new Vector3d(
(delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
-((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
(delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
);
Vector3d torque = vesselState.torqueAvailable + vesselState.torqueFromEngine * vessel.ctrlState.mainThrottle;
Vector3d inertia = Vector3d.Scale(
vesselState.angularMomentum.Sign(),
Vector3d.Scale(
Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum),
Vector3d.Scale(torque, vesselState.MoI).Invert()
)
);
// ( MoI / avaiable torque ) factor:
Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132);
// Find out the real shorter way to turn were we wan to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vessel.ReferenceTransform.transform.rotation.Inverse() * target * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0f;
Vector3d err = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
attitudeRollMatters?((delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z) * Math.PI / 180.0F : 0F
);
err += inertia.Reorder(132) / 2;
err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(NormFactor);
// angular velocity:
Vector3d omega;
omega.x = vessel.angularVelocity.x;
omega.y = vessel.angularVelocity.z; // y <=> z
omega.z = vessel.angularVelocity.y; // z <=> y
omega.Scale(NormFactor);
pidAction = pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = lastAct + (pidAction - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1));
lastAct = act;
SetFlightCtrlState(act, deltaEuler, s, 1);
act = new Vector3d(s.pitch, s.yaw, s.roll);
}
}
/// <summary>
/// Computes and returns the target vector for approach and autoland.
/// </summary>
/// <returns></returns>
public Vector3d GetAutolandTargetVector()
{
// positions of the start and end of the runway
Vector3d runwayStart = runway.GetVectorToTouchdown();
Vector3d runwayEnd = runway.End();
// get the initial and final approach vectors
Vector3d initialApproachVector = GetInitialApproachPoint();
Vector3d finalApproachVector = GetFinalApproachPoint();
// determine whether the vessel is within the approach cone or not
Vector3d finalApproachVectorProjectedOnGroundPlane = finalApproachVector.ProjectOnPlane(runway.Up());
Vector3d initialApproachVectorProjectedOnGroundPlane = initialApproachVector.ProjectOnPlane(runway.Up());
Vector3d runwayDirectionVectorProjectedOnGroundPlane = (runwayEnd - runwayStart).ProjectOnPlane(runway.Up());
double lateralAngleOfFinalApproachVector = Vector3d.Angle(finalApproachVectorProjectedOnGroundPlane, runwayDirectionVectorProjectedOnGroundPlane);
double lateralAngleOfInitialApproachVector = Vector3d.Angle(initialApproachVectorProjectedOnGroundPlane, runwayDirectionVectorProjectedOnGroundPlane);
if (approachState == AutolandApproachState.START)
{
if (lateralAngleOfFinalApproachVector < lateralApproachConeAngle)
{
// We are within the approach cone, we can skip IAP and
// instead start intercepting the glideslope.
approachState = AutolandApproachState.GLIDESLOPEINTERCEPT;
return(FindVectorToGlideslopeIntercept(finalApproachVector, lateralAngleOfFinalApproachVector));
}
approachState = AutolandApproachState.IAP;
return(initialApproachVector);
}
else if (approachState == AutolandApproachState.IAP)
{
if (lateralAngleOfInitialApproachVector > 180 - lateralApproachConeAngle)
{
// We are within the "bad" cone. We have to go all the way
// to IAP without cutting corners.
return(initialApproachVector);
}
if (lateralAngleOfFinalApproachVector < lateralApproachConeAngle)
{
// We are in the approach cone, start glideslope intercept.
approachState = AutolandApproachState.GLIDESLOPEINTERCEPT;
return(FindVectorToGlideslopeIntercept(finalApproachVector, lateralAngleOfFinalApproachVector));
}
return(initialApproachVector);
}
else if (approachState == AutolandApproachState.GLIDESLOPEINTERCEPT)
{
Vector3d vectorToGlideslopeIntercept = FindVectorToGlideslopeIntercept(finalApproachVector, lateralAngleOfFinalApproachVector);
// Determine whether we should start turning towards FAP.
double estimatedTimeToTurn = lateralInterceptAngle / GetAutolandMaxRateOfTurn();
double timeToGlideslopeIntercept = LateralDistance(vesselState.CoM, vectorToGlideslopeIntercept) / vesselState.speedSurfaceHorizontal;
if (estimatedTimeToTurn >= timeToGlideslopeIntercept)
{
approachState = AutolandApproachState.FAP;
return(finalApproachVector);
}
// Otherwise, continue flying towards the glideslope intercept.
return(vectorToGlideslopeIntercept);
}
else if (approachState == AutolandApproachState.FAP)
{
if (lateralAngleOfFinalApproachVector > lateralInterceptAngle)
{
// Cancel final approach, go back to initial approach.
approachState = AutolandApproachState.IAP;
return(initialApproachVector);
}
double timeToFAP = LateralDistance(vesselState.CoM, finalApproachVector) / vesselState.speedSurfaceHorizontal;
if (GetAutolandAlignmentError(finalApproachVector) < 3.0 && timeToFAP < secondsThresholdToNextWaypoint)
{
approachState = AutolandApproachState.TOUCHDOWN;
return(runway.GetVectorToTouchdown());
}
return(finalApproachVector);
}
else if (approachState == AutolandApproachState.TOUCHDOWN)
{
double timeToTouchdown = LateralDistance(vesselState.CoM, runway.GetVectorToTouchdown()) / vesselState.speedSurfaceHorizontal;
if (vesselState.altitudeTrue < startFlareAtAltitude + 10)
{
approachState = AutolandApproachState.WAITINGFORFLARE;
return(runway.End());
}
return(runway.GetVectorToTouchdown());
}
else if (approachState == AutolandApproachState.WAITINGFORFLARE)
{
if (vesselState.altitudeTrue < startFlareAtAltitude)
{
approachState = AutolandApproachState.FLARE;
flareStartAoA = vesselState.AoA;
}
return(runway.End());
}
else if (approachState == AutolandApproachState.FLARE)
{
if (vessel.Landed)
{
touchdownMomentAoA = vesselState.AoA;
touchdownMomentSpeed = vesselState.speedSurfaceHorizontal;
approachState = AutolandApproachState.ROLLOUT;
}
return(runway.End());
}
else if (approachState == AutolandApproachState.ROLLOUT)
{
if (vesselState.speedSurface < 1.0)
{
AutopilotOff();
// disable the actual autopilot so we dont takeoff again after we landed
Autopilot.enabled = false;
}
return(runway.End());
}
Debug.Assert(false);
return(runway.Start());
}
/// <summary>
/// This routine was originally meant to actually do the raycast entirely by itself,
/// but the KSP API method pqs.RayIntersection() does not do what it sounds like it does.
/// It only finds the sea-level intersection, not the terrain intersection. So now this
/// method is only the initial starting point of the algorithm - it just finds the sea level
/// intersect that's under the actual terrain.
/// </summary>
/// <param name="origin">Location to start the ray from</param>
/// <param name="rayVec">A vector describing the direction of the ray relative from origin</param>
/// <param name="inBody">The body to check for. Pass in null to try all the bodies in the game.</param>
/// <param name="hitBody">The body that the hit was found for (=inBody if inBody wasn't null).</param>
/// <param name="lowerBoundDist">The minimum possible distance the terrain hit might be found.</param>
/// <param name="upperBoundDist">The maximum possible distance the terrain hit might be found.</param>
/// <returns>True if there is a hit that seems likely, where the ray intersects the area between a body'd radiusMin and radiusMax.</returns>
private bool raySphereLevelCast(
Vector3d origin,
Vector3d rayVec,
CelestialBody inBody,
out CelestialBody hitBody,
out double lowerBoundDist,
out double upperBoundDist)
{
DebugMsg("raySphereLevelCast( " + origin + "," + rayVec + "," + inBody + "(out hitBody),(out dist));");
List <CelestialBody> bodies;
if (inBody == null)
{
DebugMsg("raySphereLevelCast checking all bodies.");
bodies = FlightGlobals.Bodies;
}
else
{
DebugMsg("raySphereLevelCast checking body " + inBody + ".");
bodies = new List <CelestialBody>();
bodies.Add(inBody);
}
double bestLowerHitDist = -1.0;
double bestUpperHitDist = -1.0;
CelestialBody bestHitBody = null;
double hitDist = -1.0;
double now = Planetarium.GetUniversalTime();
// For each body in the game, find if there's a hit with its surface calculator,
// and if there is, then keep the hit that's closest (just in case two
// bodies are in line of sight where a ray hits both of them, we want the
// nearer of those hits.)
foreach (CelestialBody body in bodies)
{
DebugMsg("raySphereLevelCast Now checking for " + body + ".");
PQS pqs = body.pqsController;
if (pqs != null)
{
upperBoundDist = -1;
double nearDist;
double farDist;
// The ray must at least intersect the max radius sphere of the body or it can't be a hit.
// If it does intersect the max radius sphere then the real hit must be between the two intersects
// (near and far) of that sphere.
bool upperFound = GetRayIntersectSphere(origin, rayVec, body.position, body.pqsController.radiusMax, out nearDist, out farDist);
if (upperFound)
{
upperBoundDist = farDist;
lowerBoundDist = nearDist;
// If the ray also hits the min radius of the sphere of the body, the the real hit must be bounded by
// the near hit of that intersect:
bool lowerFound = GetRayIntersectSphere(origin, rayVec, body.position, body.pqsController.radiusMin, out nearDist, out farDist);
if (lowerFound)
{
upperBoundDist = nearDist;
}
Vector3d vecToUpperBound = upperBoundDist * rayVec;
// Check to see if the hit is in front of the ray instead of behind it:
if (Vector3d.Angle(vecToUpperBound, rayVec) <= 90)
{
if (bestUpperHitDist < 0 || bestUpperHitDist > hitDist)
{
bestLowerHitDist = lowerBoundDist;
bestUpperHitDist = upperBoundDist;
bestHitBody = body;
}
}
}
}
}
hitBody = bestHitBody;
upperBoundDist = bestUpperHitDist;
lowerBoundDist = bestLowerHitDist;
bool hitFound = (upperBoundDist > 0);
if (hitFound)
{
// A hit has to be maintained steadily, long enough to to last a full lightspeed
// round trip, or it doesn't really count as a hit yet:
if (now < lastFailTime + ((upperBoundDist * 2) / lightspeed))
{
hitFound = false;
}
}
else
{
lastFailTime = now;
}
DebugMsg("raySphereLevelCast returning " + hitFound + ", out hitBody=" + hitBody + ", out dist=" + upperBoundDist);
return(hitFound);
}
private double CalcRelativeInclination(Orbit origin, Orbit target)
{
return(Vector3d.Angle(origin.GetOrbitNormal(), target.GetOrbitNormal()));
}
/// <summary>
/// Homemade routine to calculate the intersection of a ray with a sphere.
/// </summary>
/// <param name="rayStart">position of the ray's start</param>
/// <param name="rayVec">vector describing the ray's look direction</param>
/// <param name="center">position of the sphere's center</param>
/// <param name="radius">length of the sphere's radius</param>
/// <param name="nearDist">returns the distance from rayStart to the near intersect</param>
/// <param name="farDist">returns the distance from raystart to the far intersect</param>
/// <returns>True if intersect found, false if no intersect was detected (in which case nearDist and farDist are both returned as -1.0)</returns>
private bool GetRayIntersectSphere(Vector3d rayStart, Vector3d rayVec, Vector3d center, double radius, out double nearDist, out double farDist)
{
DebugMsg("GetRayIntersectSphere(" + rayStart + "," + rayVec + "," + center + "," + radius + ",(out),(out));");
// The math algorithm is explained in this long ascii art comment:
//
// (original ascii art circle copied from ascii.co.uk/art/circle)
//
// ,,----~""""~----,,
// ,---""' `""--,
// / L2 ,--!" C1 "~--, L1 L0 P0
// <--------@----------------@----------------@-------------@<==========@---
// \ ,-!" : __- "~-, | theta ___---
// ,|" : _- "|, \ ___---
// ,|' : r __- `|___---
// ,|' : _- ___---|,
// -' : __- ___--- `-
// | C0:_- ___--- |
// | @--- |
// | \ |
// | \ |
// ~, \ ,!
// `|, \ r ,|'
// `|, \ ,|'
// `|- \ |'
// `~-- \ --!'
// "~-- \ --~"
// `"~--, \ ,--!"'
// `"~|--, ,--\!"'
// ``""~~-----!!""''
//
// Knowns:
// P0 = Position of ray start
// L0 = Position of "lookat" that represents the ray direction.
// C0 = Position of sphere center
// r = radius of sphere
//
// Not known, but trivial to calculate with built-ins:
// theta = angle between the ray and a line to the sphere center.
//
// Unknowns:
// L1, L2 = the interesect points - finding these is the goal.
// C1 = The point on the ray in the center of its chord through the sphere
// It represents the point where the line from center to the ray is
// perpendicular with the ray - so it can be used to form right triangles
// which helps because it means we can use trig.
//
// Notation used (since ascii can't do some things well):
//
// _____\ = the "vector" symbol, vector from P0 to L0.
// P0.L0
//
// | | = The "absolute value" symbol, or "vector magnitude" symbol.
// | |
//
// We'll use theta to do some trig, so finding it is key. It's just the
// angle between these two vectors, which is trivial to calculate with
// a Unity built-in or with a dot-product:
//
// theta = angle between | _____\ | and | _____\ |
// | P0.L0 | | P0.C0 |
//
//
// Length of the two legs of the large triangle can be found by simple trig:
//
// | _____\ | | _____\ |
// | C0.C1 | = sin(theta) * | P0.C0 |
//
// | _____\ | | _____\ |
// | P0.C1 | = cos(theta) * | P0.C0 |
//
// At this point if the length of C0.C1 is bigger than r, we can abort here
// as there are no solutions, because it means the ray is entirely outside
// the sphere.
//
// Length of the distance from C1 to L1 can be found by Pythagoras A^2 + B^2 = C^2:
//
// | _____\ |
// | C0.L1 | = r (just the radius of the sphere)
//
// .------------------------
// | _____\ | /\ / 2 | _____\ | 2
// | C1.L1 | = \/ r - | C0.C1 |
//
//
// With that, we know the lengths needed:
//
// | _____\ | | _____\ | | _____\ |
// | P0.L1 | = | P0.C1 | - | C1.L1 |
//
// | _____\ | | _____\ | | _____\ |
// | P0.L2 | = | P0.C1 | + | C1.L1 |
//
// Knowing those two lengths is really the point of the algorithm. Getting
// the actual points in question is just a matter of multiplying them by the
// lookat unit vector, but that's left for the caller to do.
double thetaRadians = Vector3d.Angle(rayVec, center - rayStart) * 0.0174532925; // 0.0174532925 = Pi/180
double lengthHypotenuse = (center - rayStart).magnitude;
double lengthC0ToC1 = Math.Sin(thetaRadians) * lengthHypotenuse;
DebugMsg("GetRayIntersectSphere: thetaRadians=" + thetaRadians + " lengthHyp=" + lengthHypotenuse + " lengthC0ToC1=" + lengthC0ToC1);
if (lengthC0ToC1 > radius)
{
nearDist = -1.0;
farDist = -1.0;
DebugMsg("GetRayIntersectSphere returning: " + false + ", nearDist=" + nearDist + ", farDist=" + farDist);
return(false);
}
double lengthP0ToC1 = Math.Cos(thetaRadians) * lengthHypotenuse;
double lengthC1ToL1 = Math.Sqrt(Math.Pow(radius, 2) - Math.Pow(lengthC0ToC1, 2));
DebugMsg("GetRayIntersectSphere: legnthP0ToC1=" + lengthP0ToC1 + " lengthC1ToL1=" + lengthC1ToL1);
nearDist = lengthP0ToC1 - lengthC1ToL1;
farDist = lengthP0ToC1 + lengthC1ToL1;
DebugMsg("GetRayIntersectSphere returning: " + true + ", nearDist=" + nearDist + ", farDist=" + farDist);
return(true);
}
public override AutopilotStep Drive(FlightCtrlState s)
{
if (vessel.LandedOrSplashed)
{
core.landing.StopLanding();
return(null);
}
// TODO perhaps we should pop the parachutes at this point, or at least consider it depending on the altitude.
double minalt = Math.Min(vesselState.altitudeBottom, Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue));
if (vesselState.limitedMaxThrustAccel < vesselState.gravityForce.magnitude)
{
// if we have TWR < 1, just try as hard as we can to decelerate:
// (we need this special case because otherwise the calculations spit out NaN's)
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL;
core.thrust.trans_kill_h = true;
core.thrust.trans_spd_act = 0;
}
else if (minalt > 200)
{
if ((vesselState.surfaceVelocity.magnitude > 5) && (Vector3d.Angle(vesselState.surfaceVelocity, vesselState.up) < 80))
{
// if we have positive vertical velocity, point up and don't thrust:
core.attitude.attitudeTo(Vector3d.up, AttitudeReference.SURFACE_NORTH, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT;
core.thrust.trans_spd_act = 0;
}
else if ((vesselState.surfaceVelocity.magnitude > 5) && (Vector3d.Angle(vesselState.forward, -vesselState.surfaceVelocity) > 45))
{
// if we're not facing approximately retrograde, turn to point retrograde and don't thrust:
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT;
core.thrust.trans_spd_act = 0;
}
else
{
//if we're above 200m, point retrograde and control surface velocity:
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE;
//core.thrust.trans_spd_act = (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * minalt) * 0.90F;
Vector3d estimatedLandingPosition = vesselState.CoM + vesselState.surfaceVelocity.sqrMagnitude / (2 * vesselState.limitedMaxThrustAccel) * vesselState.surfaceVelocity.normalized;
double terrainRadius = mainBody.Radius + mainBody.TerrainAltitude(estimatedLandingPosition);
aggressivePolicy = new GravityTurnDescentSpeedPolicy(terrainRadius, mainBody.GeeASL * 9.81, vesselState.limitedMaxThrustAccel); // this constant policy creation is wastefull...
core.thrust.trans_spd_act = (float)(aggressivePolicy.MaxAllowedSpeed(vesselState.CoM - mainBody.position, vesselState.surfaceVelocity));
thrustAt200Meters = (float)vesselState.limitedMaxThrustAccel; // this gets updated until we are below 200 meters
}
}
else
{
float previous_trans_spd_act = Math.Abs(core.thrust.trans_spd_act); // avoid issues going from KEEP_SURFACE mode to KEEP_VERTICAL mode
// Last 200 meters, at this point the rocket has a TWR that will rise rapidly, so be sure to use the same policy based on an older
// TWR. Otherwise we would suddenly get a large jump in desired speed leading to a landing that is too fast.
core.thrust.trans_spd_act = Mathf.Lerp(0, (float)Math.Sqrt((thrustAt200Meters - vesselState.localg) * 2 * 200) * 0.90F, (float)minalt / 200);
// Sometimes during the descent we end up going up a bit due to overthrusting during breaking, avoid thrusting up even more and destabilizing the rocket
core.thrust.trans_spd_act = (float)Math.Min(previous_trans_spd_act, core.thrust.trans_spd_act);
// take into account desired landing speed:
core.thrust.trans_spd_act = (float)Math.Max(core.landing.touchdownSpeed, core.thrust.trans_spd_act);
// Prevent that we switch back from Vertical mode to KeepSurface mode
// When that happens the rocket will start tilting and end up falling over
if (vesselState.speedSurfaceHorizontal < 5)
{
forceVerticalMode = true;
}
if (forceVerticalMode)
{
// if we're falling more or less straight down, control vertical speed and
// kill horizontal velocity
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL;
core.thrust.trans_spd_act *= -1;
core.thrust.trans_kill_h = false; // rockets are long, and will fall over if you attempt to do any manouvers to kill that last bit of horizontal speed
if (core.landing.rcsAdjustment) // If allowed, use RCS to stablize the rocket
{
core.rcs.enabled = true;
}
// Turn on SAS because we are likely to have a bit of horizontal speed left that needs to stabilize
// Use SmartASS, because Mechjeb doesn't expect SAS to be used (i.e. it is automatically turned off again)
core.EngageSmartASSControl(MechJebModuleSmartASS.Mode.SURFACE, MechJebModuleSmartASS.Target.VERTICAL_PLUS, true);
}
else
{
// if we're falling at a significant angle from vertical, our vertical speed might be
// quite small but we might still need to decelerate. Control the total speed instead
// by thrusting directly retrograde
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null);
core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE;
}
}
status = "Final descent: " + vesselState.altitudeBottom.ToString("F0") + "m above terrain";
// ComputeCourseCorrection doesn't work close to the ground
/* if (core.landing.landAtTarget)
* {
* core.rcs.enabled = true;
* Vector3d deltaV = core.landing.ComputeCourseCorrection(false);
* core.rcs.SetWorldVelocityError(deltaV);
*
* status += "\nDV: " + deltaV.magnitude.ToString("F3") + " m/s";
* } */
return(this);
}
//--------------------------------------------------------------------
// CheckDraw
// Checks if the given mesh should be drawn.
private void CheckDraw(GameObject flareMesh, MeshRenderer flareMR, Vector3d position, CelestialBody referenceBody, Vector4 hslColor, double objRadius, FlareType flareType)
{
Vector3d targetVectorToSun = FlightGlobals.Bodies[0].position - position;
Vector3d targetVectorToRef = referenceBody.position - position;
double targetRelAngle = Vector3d.Angle(targetVectorToSun, targetVectorToRef);
double targetDist = Vector3d.Distance(position, camPos);
double targetSize;
if (flareType == FlareType.Celestial)
{
targetSize = objRadius;
}
else
{
targetSize = Math.Atan2(objRadius, targetDist) * Mathf.Rad2Deg;
}
double targetRefDist = Vector3d.Distance(position, referenceBody.position);
double targetRefSize = Math.Acos(Math.Sqrt(Math.Pow(targetRefDist, 2.0) - Math.Pow(referenceBody.Radius, 2.0)) / targetRefDist) * Mathf.Rad2Deg;
bool inShadow = false;
if (referenceBody != FlightGlobals.Bodies[0] && targetRelAngle < targetRefSize)
{
inShadow = true;
}
bool isVisible;
if (inShadow)
{
isVisible = false;
}
else
{
isVisible = true;
// See if the sun obscures our target
if (sunDistanceFromCamera < targetDist && sunSizeInDegrees > targetSize && Vector3d.Angle(cameraToSunUnitVector, position - camPos) < sunSizeInDegrees)
{
isVisible = false;
}
if (isVisible)
{
for (int i = 0; i < bodyFlares.Count; ++i)
{
if (bodyFlares[i].body.bodyName != flareMesh.name && bodyFlares[i].distanceFromCamera <targetDist && bodyFlares[i].sizeInDegrees> targetSize && Vector3d.Angle(bodyFlares[i].cameraToBodyUnitVector, position - camPos) < bodyFlares[i].sizeInDegrees)
{
isVisible = false;
break;
}
}
}
}
if (targetSize < (camFOV / 500.0f) && isVisible && !MapView.MapIsEnabled)
{
// Work in HSL space. That allows us to do dimming of color
// by adjusting the lightness value without any hue shifting.
// We apply atmospheric dimming using alpha. Although maybe
// I don't need to - it could be done by dimming, too.
float alpha = hslColor.w;
float dimming = 1.0f;
alpha *= atmosphereFactor;
dimming *= dimFactor;
if (targetSize > (camFOV / 1000.0f))
{
dimming *= (float)(((camFOV / targetSize) / 500.0) - 1.0);
}
if (flareType == FlareType.Debris && DistantObjectSettings.DistantFlare.debrisBrightness < 1.0f)
{
dimming *= DistantObjectSettings.DistantFlare.debrisBrightness;
}
// Uncomment this to help with debugging
//alpha = 1.0f;
//dimming = 1.0f;
flareMR.material.color = ResourceUtilities.HSL2RGB(hslColor.x, hslColor.y, hslColor.z * dimming, alpha);
}
else
{
flareMesh.SetActive(false);
}
}
public void CalculateNewOrbitData()
{
isDirty = false;
var MG = attractorMass * gravConst;
attractorDistance = position.magnitude;
var angularMomentumVector = CelestialBodyUtils.CrossProduct(position, velocity);
orbitNormal = angularMomentumVector.normalized;
Vector3d eccVector;
if (orbitNormal.sqrMagnitude < 0.9 || orbitNormal.sqrMagnitude > 1.1) //check if zero lenght
{
orbitNormal = CelestialBodyUtils.CrossProduct(position, eclipticUp).normalized;
eccVector = new Vector3d();
}
else
{
eccVector = CelestialBodyUtils.CrossProduct(velocity, angularMomentumVector) / MG - position / attractorDistance;
}
orbitNormalDotEclipticNormal = CelestialBodyUtils.DotProduct(orbitNormal, eclipticNormal);
focalParameter = angularMomentumVector.sqrMagnitude / MG;
eccentricity = eccVector.magnitude;
//if (debug) {
// string format = "0.0000000000";
// Debug.Log(
// "ECC: " + eccVector.ToString(format) + " LEN: " + eccVector.magnitude.ToString(format) + "\n" +
// "POS: " + position.ToString(format) + " LEN: " + position.magnitude.ToString(format) + "\n" +
// "POSNORM: " + ( position / attractorDistance ).ToString(format) + " LEN: " + ( position / attractorDistance ).magnitude.ToString(format) + "\n" +
// "VEL: " + velocity.ToString(format) + " LEN: " + velocity.magnitude.ToString(format) + "\n" +
// "POScrossVEL: " + angularMomentumVector.ToString(format) + " LEN: " + angularMomentumVector.magnitude.ToString(format) + "\n"
// );
//}
energyTotal = velocity.sqrMagnitude - 2 * MG / attractorDistance;
semiMinorAxisBasis = CelestialBodyUtils.CrossProduct(angularMomentumVector, eccVector).normalized;
if (semiMinorAxisBasis.sqrMagnitude < 0.5)
{
semiMinorAxisBasis = CelestialBodyUtils.CrossProduct(orbitNormal, position).normalized;
}
semiMajorAxisBasis = CelestialBodyUtils.CrossProduct(orbitNormal, semiMinorAxisBasis).normalized;
inclination = Vector3d.Angle(orbitNormal, eclipticNormal) * Mathd.Deg2Rad;
if (eccentricity < 1)
{
orbitCompressionRatio = 1 - eccentricity * eccentricity;
semiMajorAxis = focalParameter / orbitCompressionRatio;
semiMinorAxis = semiMajorAxis * System.Math.Sqrt(orbitCompressionRatio);
centerPoint = -semiMajorAxis * eccVector;
period = Mathd.PI_2 * Mathd.Sqrt(Mathd.Pow(semiMajorAxis, 3) / MG);
apoapsis = centerPoint + semiMajorAxisBasis * semiMajorAxis;
periapsis = centerPoint - semiMajorAxisBasis * semiMajorAxis;
periapsisDistance = periapsis.magnitude;
apoapsisDistance = apoapsis.magnitude;
trueAnomaly = Vector3d.Angle(position, -semiMajorAxisBasis) * Mathd.Deg2Rad;
if (CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(position, semiMajorAxisBasis), orbitNormal) < 0)
{
trueAnomaly = Mathd.PI_2 - trueAnomaly;
}
eccentricAnomaly = CelestialBodyUtils.ConvertTrueToEccentricAnomaly(trueAnomaly, eccentricity);
meanAnomaly = eccentricAnomaly - eccentricity * System.Math.Sin(eccentricAnomaly);
}
else
{
orbitCompressionRatio = eccentricity * eccentricity - 1;
semiMajorAxis = focalParameter / orbitCompressionRatio;
semiMinorAxis = semiMajorAxis * System.Math.Sqrt(orbitCompressionRatio);
centerPoint = semiMajorAxis * eccVector;
period = double.PositiveInfinity;
apoapsis = new Vector3d(double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity);
periapsis = centerPoint + semiMajorAxisBasis * (semiMajorAxis);
periapsisDistance = periapsis.magnitude;
apoapsisDistance = double.PositiveInfinity;
trueAnomaly = Vector3d.Angle(position, eccVector) * Mathd.Deg2Rad;
if (CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(position, semiMajorAxisBasis), orbitNormal) < 0)
{
trueAnomaly = -trueAnomaly;
}
eccentricAnomaly = CelestialBodyUtils.ConvertTrueToEccentricAnomaly(trueAnomaly, eccentricity);
meanAnomaly = System.Math.Sinh(eccentricAnomaly) * eccentricity - eccentricAnomaly;
}
}
//--------------------------------------------------------------------
// UpdateVar()
// Update atmosphereFactor and dimFactor
private void UpdateVar()
{
Vector3d sunBodyAngle = (FlightGlobals.Bodies[0].position - camPos).normalized;
double sunBodyDist = FlightGlobals.Bodies[0].GetAltitude(camPos) + FlightGlobals.Bodies[0].Radius;
double sunBodySize = Math.Acos(Math.Sqrt(Math.Pow(sunBodyDist, 2.0) - Math.Pow(FlightGlobals.Bodies[0].Radius, 2.0)) / sunBodyDist) * Mathf.Rad2Deg;
atmosphereFactor = 1.0f;
if (FlightGlobals.currentMainBody != null && FlightGlobals.currentMainBody.atmosphere)
{
double camAltitude = FlightGlobals.currentMainBody.GetAltitude(camPos);
double atmAltitude = FlightGlobals.currentMainBody.atmosphereDepth;
double atmCurrentBrightness = (Vector3d.Distance(camPos, FlightGlobals.Bodies[0].position) - Vector3d.Distance(FlightGlobals.currentMainBody.position, FlightGlobals.Bodies[0].position)) / (FlightGlobals.currentMainBody.Radius);
if (camAltitude > (atmAltitude / 2.0) || atmCurrentBrightness > 0.15)
{
atmosphereFactor = 1.0f;
}
else if (camAltitude < (atmAltitude / 10.0) && atmCurrentBrightness < 0.05)
{
atmosphereFactor = 0.0f;
}
else
{
if (camAltitude < (atmAltitude / 2.0) && camAltitude > (atmAltitude / 10.0) && atmCurrentBrightness < 0.15)
{
atmosphereFactor *= (float)((camAltitude - (atmAltitude / 10.0)) / (atmAltitude - (atmAltitude / 10.0)));
}
if (atmCurrentBrightness < 0.15 && atmCurrentBrightness > 0.05 && camAltitude < (atmAltitude / 2.0))
{
atmosphereFactor *= (float)((atmCurrentBrightness - 0.05) / (0.10));
}
if (atmosphereFactor > 1.0f)
{
atmosphereFactor = 1.0f;
}
}
// atmDensityASL isn't an exact match for atmosphereMultiplier from KSP 0.90, I think, but it
// provides a '1' for Kerbin (1.2, actually)
float atmThickness = (float)Math.Min(Math.Sqrt(FlightGlobals.currentMainBody.atmDensityASL), 1);
atmosphereFactor = (atmThickness) * (atmosphereFactor) + (1.0f - atmThickness);
}
float sunDimFactor = 1.0f;
float skyboxDimFactor;
if (DistantObjectSettings.SkyboxBrightness.changeSkybox == true)
{
// Apply fudge factors here so people who turn off the skybox don't turn off the flares, too.
// And avoid a divide-by-zero.
skyboxDimFactor = Mathf.Max(0.5f, GalaxyCubeControl.Instance.maxGalaxyColor.r / Mathf.Max(0.0078125f, DistantObjectSettings.SkyboxBrightness.maxBrightness));
}
else
{
skyboxDimFactor = 1.0f;
}
// This code applies a fudge factor to flare dimming based on the
// angle between the camera and the sun. We need to do this because
// KSP's sun dimming effect is not applied to maxGalaxyColor, so we
// really don't know how much dimming is being done.
float angCamToSun = Vector3.Angle(FlightCamera.fetch.mainCamera.transform.forward, sunBodyAngle);
if (angCamToSun < (camFOV * 0.5f))
{
bool isVisible = true;
for (int i = 0; i < bodyFlares.Count; ++i)
{
if (bodyFlares[i].distanceFromCamera <sunBodyDist && bodyFlares[i].sizeInDegrees> sunBodySize && Vector3d.Angle(bodyFlares[i].cameraToBodyUnitVector, FlightGlobals.Bodies[0].position - camPos) < bodyFlares[i].sizeInDegrees)
{
isVisible = false;
break;
}
}
if (isVisible)
{
// Apply an arbitrary minimum value - the (x^4) function
// isn't right, but it does okay on its own.
float sunDimming = Mathf.Max(0.2f, Mathf.Pow(angCamToSun / (camFOV * 0.5f), 4.0f));
sunDimFactor *= sunDimming;
}
}
dimFactor = DistantObjectSettings.DistantFlare.flareBrightness * Mathf.Min(skyboxDimFactor, sunDimFactor);
}
//angle in degrees between the vessel's current pointing direction and the attitude target, ignoring roll
public double attitudeAngleFromTarget()
{
return(enabled ? Math.Abs(Vector3d.Angle(attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Vector3d.forward, vesselState.forward)) : 0);
}
//--------------------------------------------------------------------
// UpdateNameShown
// Update the mousever name (if applicable)
private void UpdateNameShown()
{
showNameTransform = null;
if (DistantObjectSettings.DistantFlare.showNames)
{
Ray mouseRay = FlightCamera.fetch.mainCamera.ScreenPointToRay(Input.mousePosition);
// Detect CelestialBody mouseovers
double bestRadius = -1.0;
foreach (BodyFlare bodyFlare in bodyFlares)
{
if (bodyFlare.body == FlightGlobals.ActiveVessel.mainBody)
{
continue;
}
if (bodyFlare.meshRenderer.material.color.a > 0.0f)
{
Vector3d vectorToBody = bodyFlare.body.position - mouseRay.origin;
double mouseBodyAngle = Vector3d.Angle(vectorToBody, mouseRay.direction);
if (mouseBodyAngle < 1.0)
{
if (bodyFlare.body.Radius > bestRadius)
{
double distance = Vector3d.Distance(FlightCamera.fetch.mainCamera.transform.position, bodyFlare.body.position);
double angularSize = Mathf.Rad2Deg * bodyFlare.body.Radius / distance;
if (angularSize < 0.2)
{
bestRadius = bodyFlare.body.Radius;
showNameTransform = bodyFlare.body.transform;
showNameString = KSP.Localization.Localizer.Format("<<1>>", bodyFlare.body.bodyDisplayName);
showNameColor = bodyFlare.color;
}
}
}
}
}
if (showNameTransform == null)
{
// Detect Vessel mouseovers
float bestBrightness = 0.01f; // min luminosity to show vessel name
foreach (VesselFlare vesselFlare in vesselFlares.Values)
{
if (vesselFlare.flareMesh.activeSelf && vesselFlare.meshRenderer.material.color.a > 0.0f)
{
Vector3d vectorToVessel = vesselFlare.referenceShip.transform.position - mouseRay.origin;
double mouseVesselAngle = Vector3d.Angle(vectorToVessel, mouseRay.direction);
if (mouseVesselAngle < 1.0)
{
float brightness = vesselFlare.brightness;
if (brightness > bestBrightness)
{
bestBrightness = brightness;
showNameTransform = vesselFlare.referenceShip.transform;
showNameString = vesselFlare.referenceShip.vesselName;
showNameColor = Color.white;
}
}
}
}
}
}
}
//Computes the angle between two orbital planes. This will be a number between 0 and 180
//Note that in the convention used two objects orbiting in the same plane but in
//opposite directions have a relative inclination of 180 degrees.
public static double RelativeInclination(this Orbit a, Orbit b)
{
return(Math.Abs(Vector3d.Angle(a.SwappedOrbitNormal(), b.SwappedOrbitNormal())));
}
public void FixedUpdate()
{
if (vesselGuid != Guid.Empty)
{
missionTime = Math.Max(missionTime, vessel.missionTime);
//Log.Info("Old vesselGuid: " + vesselGuid + ", missiontime: " + missionTime);
}
else
{
vesselGuid = this.vessel.id;
//Log.Info("vesselGuid: " + vesselGuid);
//Log.Info("guid missionTime: " + vessel.missionTime);
}
if (HighLogic.LoadedSceneIsFlight)
{
missionTime = vessel.missionTime;
var vm = this;
//Vessel v = lastActiveVessel;
Vessel v = this.vessel;
//if (v != lastActiveVessel)
//{
// lastActiveVessel = v;
// vm = v.GetComponent<BPB_VesselModule>();
//}
//if (vm.armed && !vm.aborted && !v.ActionGroups[KSPActionGroup.Abort])
if (armed && !aborted && !v.ActionGroups[KSPActionGroup.Abort] && missionTime > 0)
{
// Check to be sure it should still be active
//if (v.missionTime <= vm.disableAfter && v.altitude <= vm.disableAtAltitudeKm * 1000)
//if (v.missionTime <= disableAfter && v.altitude <= disableAtAltitudeKm * 1000)
if (this.missionTime <= disableAfter && v.altitude <= disableAtAltitude)
{
// check for negative vertical speed
//if (v.verticalSpeed < vm.vertSpeed && vm.vertSpeedTriggerEnabled)
if (v.verticalSpeed < vertSpeed && vertSpeedTriggerEnabled)
{
//if (this.vessel == FlightGlobals.ActiveVessel)
if (this.vessel.isActiveVessel)
{
ScreenMessages.PostScreenMessage("<color=red>ABORTING - Negative Vertical Velocity Detected!</color>", 10f);
}
else
{
ScreenMessages.PostScreenMessage("ABORTING - Negative Vertical Velocity Detected!");
}
ScreenMessages.PostScreenMessage(v.verticalSpeed + " m/s", 10f);
v.ActionGroups.SetGroup(KSPActionGroup.Abort, true);
//vm.SetAllActive(true, false, "Aborted! Negative Vertical Velocity Detected");
SetAllActive(true, false, "Aborted! Negative Vertical Velocity Detected");
}
// Check for G forces too high
//if (v.geeForce > vm.gForceTrigger && vm.gForceTriggerEnabled)
if (v.geeForce > gForceTrigger && gForceTriggerEnabled)
{
//if (this.vessel == FlightGlobals.ActiveVessel)
if (this.vessel.isActiveVessel)
{
ScreenMessages.PostScreenMessage("<color=red>ABORTING - High G-Force Detected!</color>", 10f);
}
else
{
ScreenMessages.PostScreenMessage("ABORTING - High G-Force Detected!");
}
ScreenMessages.PostScreenMessage(v.geeForce + " Gs", 10f);
v.ActionGroups.SetGroup(KSPActionGroup.Abort, true);
vm.SetAllActive(true, false, "Aborted! High G-Force Detected");
}
// Check for AoA too high. Also make sure that the velocity is >10 to avoid the
// inevitable jitter before launch
//if (vm.exceedingAoA && v.GetSrfVelocity().magnitude > 10 && v.missionTime > 1)
if (vm.exceedingAoA && v.GetSrfVelocity().magnitude > 10 && missionTime > 10 && v.altitude <= vm.ignoreAoAAboveAltitude)
{
var v3d1 = Vector3d.Angle(v.GetTransform().up, v.GetSrfVelocity());
if (v3d1 > vm.maxAoA)
{
//if (this.vessel == FlightGlobals.ActiveVessel)
if (this.vessel.isActiveVessel)
{
ScreenMessages.PostScreenMessage("<color=red>ABORTING - Max AoA Exceeded!</color>", 10f);
}
else
{
ScreenMessages.PostScreenMessage("ABORTING - Max AoA Exceeded!");
}
ScreenMessages.PostScreenMessage(vm.maxAoA + " degrees", 10f);
v.ActionGroups.SetGroup(KSPActionGroup.Abort, true);
vm.SetAllActive(true, false, "Aborted! Max AoA Exceeded");
}
}
}
else
{
if (!timeoutInProgress)
{
timeoutInProgress = true;
}
if (timeoutInProgress && v.geeForce <= vm.maxTimeoutActionG)
{
vm.armed = false;
timeoutInProgress = false;
//if (v.missionTime > vm.disableAfter)
if (this.missionTime > vm.disableAfter)
{
//if (this.vessel == FlightGlobals.ActiveVessel)
if (this.vessel.isActiveVessel)
{
ScreenMessages.PostScreenMessage("Bob's Panic Box disabled due to timeout", 10f);
}
Log.Info("Bob's Panic Box disabled due to timeout");
}
if (v.altitude > vm.disableAtAltitude)
{
//if (this.vessel == FlightGlobals.ActiveVessel)
if (this.vessel.isActiveVessel)
{
ScreenMessages.PostScreenMessage("Bob's Panic Box disabled due to altitude", 10f);
}
Log.Info("Bob's Panic Box disabled due to altitude");
}
if (vm.actionAfterTimeout > 0)
{
var kg = GetActionGroup((int)vm.actionAfterTimeout);
v.ActionGroups.SetGroup(kg, true);
}
}
}
}
if (vm.aborted && vm.postAbortAction != 0 && !vm.postAbortActionCompleted)
{
//if (vm.abortTime + vm.postAbortDelay <= v.missionTime)
if (vm.abortTime + vm.postAbortDelay <= this.missionTime)
{
// Check for safechute
// check for chutes
var pa = v.FindPartModulesImplementing <ModuleParachute>();
if (pa != null && pa.Count > 0)
{
foreach (ModuleParachute chute in pa)
{
if (chute.deploymentState == ModuleParachute.deploymentStates.STOWED && FlightGlobals.ActiveVessel.atmDensity > 0)
{
if (chute.deploySafe == "Safe")
{
var kg = GetActionGroup(vm.postAbortAction);
v.ActionGroups.SetGroup(kg, true);
vm.postAbortActionCompleted = true;
}
}
}
}
else
{
var kg = GetActionGroup(vm.postAbortAction);
v.ActionGroups.SetGroup(kg, true);
vm.postAbortActionCompleted = true;
}
}
}
}
}