//Computes the heading of the ground track of an orbit with a given inclination at a given latitude.
//Both inputs are in degrees.
//Convention: At equator, inclination 0 => heading 90 (east)
// inclination 90 => heading 0 (north)
// inclination -90 => heading 180 (south)
// inclination ±180 => heading 270 (west)
//Returned heading is in degrees and in the range 0 to 360.
//If the given latitude is too large, so that an orbit with a given inclination never attains the
//given latitude, then this function returns either 90 (if -90 < inclination < 90) or 270.
public static double HeadingForInclination(double inclinationDegrees, double latitudeDegrees)
{
double cosDesiredSurfaceAngle = Math.Cos(inclinationDegrees * Math.PI / 180) / Math.Cos(latitudeDegrees * Math.PI / 180);
if (Math.Abs(cosDesiredSurfaceAngle) > 1.0)
{
//If inclination < latitude, we get this case: the desired inclination is impossible
if (Math.Abs(MuUtils.ClampDegrees180(inclinationDegrees)) < 90)
{
return(90);
}
else
{
return(270);
}
}
else
{
double angleFromEast = (180 / Math.PI) * Math.Acos(cosDesiredSurfaceAngle); //an angle between 0 and 180
if (inclinationDegrees < 0)
{
angleFromEast *= -1;
}
//now angleFromEast is between -180 and 180
return(MuUtils.ClampDegrees360(90 - angleFromEast));
}
}
public override void OnFixedUpdate()
{
if (markUT == 0)
{
Mark();
}
timeSinceMark = vesselState.time - markUT;
if (vessel.situation == Vessel.Situations.PRELAUNCH)
{
Mark(); //keep resetting stats until we launch
return;
}
gravityLosses += vesselState.deltaT * Vector3d.Dot(-vessel.srf_velocity.normalized, vesselState.gravityForce);
gravityLosses -= vesselState.deltaT * Vector3d.Dot(vessel.srf_velocity.normalized, vesselState.up * vesselState.radius * Math.Pow(2 * Math.PI / part.vessel.mainBody.rotationPeriod, 2));
double dragAccel = mainBody.DragAccel(vesselState.CoM, vessel.obt_velocity, vesselState.massDrag / vesselState.mass).magnitude;
dragLosses += vesselState.deltaT * dragAccel;
maxDragGees = Math.Max(maxDragGees, dragAccel / 9.81);
double circularPeriod = 2 * Math.PI * vesselState.radius / OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, vesselState.radius);
double angleTraversed = (vesselState.longitude - markLongitude) + 360 * (vesselState.time - markUT) / part.vessel.mainBody.rotationPeriod;
phaseAngleFromMark = MuUtils.ClampDegrees360(360 * (vesselState.time - markUT) / circularPeriod - angleTraversed);
}
//Interprets a given AbsoluteVector as a velocity, and returns the corresponding Vector3d velocity
//in world coordinates.
public Vector3d WorldVelocityAtCurrentTime(AbsoluteVector absolute)
{
double now = Planetarium.GetUniversalTime();
double unrotatedLongitude = MuUtils.ClampDegrees360(absolute.longitude - 360 * (now - absolute.UT) / referenceBody.rotationPeriod);
return(absolute.radius * referenceBody.GetSurfaceNVector(absolute.latitude, unrotatedLongitude));
}
//TODO 1.1 changed trueAnomaly to rad but MJ ext stil uses deg. Should change for consistency
//Originally by Zool, revised by The_Duck
//Converts a true anomaly into an eccentric anomaly.
//For elliptical orbits this returns a value between 0 and 2pi
//For hyperbolic orbits the returned value can be any number.
//NOTE: For a hyperbolic orbit, if a true anomaly is requested that does not exist (a true anomaly
//past the true anomaly of the asymptote) then an ArgumentException is thrown
public static double GetEccentricAnomalyAtTrueAnomaly(this Orbit o, double trueAnomaly)
{
double e = o.eccentricity;
trueAnomaly = MuUtils.ClampDegrees360(trueAnomaly);
trueAnomaly = trueAnomaly * (Math.PI / 180);
if (e < 1) //elliptical orbits
{
double cosE = (e + Math.Cos(trueAnomaly)) / (1 + e * Math.Cos(trueAnomaly));
double sinE = Math.Sqrt(1 - (cosE * cosE));
if (trueAnomaly > Math.PI)
{
sinE *= -1;
}
return(MuUtils.ClampRadiansTwoPi(Math.Atan2(sinE, cosE)));
}
else //hyperbolic orbits
{
double coshE = (e + Math.Cos(trueAnomaly)) / (1 + e * Math.Cos(trueAnomaly));
if (coshE < 1)
{
throw new ArgumentException("OrbitExtensions.GetEccentricAnomalyAtTrueAnomaly: True anomaly of " + trueAnomaly + " radians is not attained by orbit with eccentricity " + o.eccentricity);
}
double E = MuUtils.Acosh(coshE);
if (trueAnomaly > Math.PI)
{
E *= -1;
}
return(E);
}
}
//Computes the time until the phase angle between the launchpad and the target equals the given angle.
//The convention used is that phase angle is the angle measured starting at the target and going east until
//you get to the launchpad.
//The time returned will not be exactly accurate unless the target is in an exactly circular orbit. However,
//the time returned will go to exactly zero when the desired phase angle is reached.
public static double TimeToPhaseAngle(double phaseAngle, CelestialBody launchBody, double launchLongitude, Orbit target)
{
double launchpadAngularRate = 360 / launchBody.rotationPeriod;
double targetAngularRate = 360.0 / target.period;
if (Vector3d.Dot(target.SwappedOrbitNormal(), launchBody.angularVelocity) < 0)
{
targetAngularRate *= -1; //retrograde target
}
Vector3d currentLaunchpadDirection = launchBody.GetSurfaceNVector(0, launchLongitude);
Vector3d currentTargetDirection = target.SwappedRelativePositionAtUT(Planetarium.GetUniversalTime());
currentTargetDirection = Vector3d.Exclude(launchBody.angularVelocity, currentTargetDirection);
double currentPhaseAngle = Math.Abs(Vector3d.Angle(currentLaunchpadDirection, currentTargetDirection));
if (Vector3d.Dot(Vector3d.Cross(currentTargetDirection, currentLaunchpadDirection), launchBody.angularVelocity) < 0)
{
currentPhaseAngle = 360 - currentPhaseAngle;
}
double phaseAngleRate = launchpadAngularRate - targetAngularRate;
double phaseAngleDifference = MuUtils.ClampDegrees360(phaseAngle - currentPhaseAngle);
if (phaseAngleRate < 0)
{
phaseAngleRate *= -1;
phaseAngleDifference = 360 - phaseAngleDifference;
}
return(phaseAngleDifference / phaseAngleRate);
}
public override void OnFixedUpdate()
{
if (markUT == 0)
{
Mark();
}
timeSinceMark = vesselState.time - markUT;
if (vessel.situation == Vessel.Situations.PRELAUNCH)
{
Mark(); //keep resetting stats until we launch
return;
}
if (vesselState.currentThrustAccel > 0)
{
gravityLosses += vesselState.deltaT * Vector3d.Dot(-vesselState.orbitalVelocity.normalized, vesselState.gravityForce);
}
dragLosses += vesselState.deltaT * vesselState.drag;
deltaVExpended += vesselState.deltaT * vesselState.currentThrustAccel;
steeringLosses += vesselState.deltaT * vesselState.currentThrustAccel * (1 - Vector3d.Dot(vesselState.orbitalVelocity.normalized, vesselState.forward));
maxDragGees = Math.Max(maxDragGees, vesselState.drag / 9.81);
double circularPeriod = 2 * Math.PI * vesselState.radius / OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, vesselState.radius);
double angleTraversed = (vesselState.longitude - markLongitude) + 360 * (vesselState.time - markUT) / part.vessel.mainBody.rotationPeriod;
phaseAngleFromMark = MuUtils.ClampDegrees360(360 * (vesselState.time - markUT) / circularPeriod - angleTraversed);
if (paused)
{
return;
}
//int oldHistoryIdx = historyIdx;
//historyIdx = Mathf.Min(Mathf.FloorToInt((float)(timeSinceMark / precision)), history.Length - 1);
if (vesselState.time >= (lastRecordTime + precision) && historyIdx < history.Length - 1)
{
lastRecordTime = vesselState.time;
historyIdx++;
Record(historyIdx);
#if DEBUG
if (TimeWarp.WarpMode == TimeWarp.Modes.HIGH)
{
Log.dbg("WRP {0} {1:0} {2:0.00}", historyIdx, history[historyIdx].downRange, history[historyIdx].AoA);
}
else
{
Log.dbg("STD {0} {1:0} {2:0.00}", historyIdx, history[historyIdx].downRange, history[historyIdx].AoA);
}
#endif
}
}
// provides AoA limiting and ground track steering to pitch controllers (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 = OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, autopilot.desiredInclination);
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;
thrustVectorForNavball = desiredThrustVector;
desiredThrustVector = desiredThrustVector.normalized;
if (autopilot.limitAoA)
{
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;
}
}
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.turnRoll, this);
}
else
{
core.attitude.attitudeTo(hdg, pitch, autopilot.verticalRoll, this);
}
}
else
{
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
}
}
/// <summary>
/// Find the time to a target plane defined by the LAN and inc for a rocket on the ground.
/// </summary>
///
/// <param name="rotationPeriod">Rotation period of the central body (seconds).</param>
/// <param name="latitude">Latitude of the launchite (degrees).</param>
/// <param name="celestialLongitude">Celestial longitude of the current position of the launchsite.</param>
/// <param name="LAN">Longitude of the Ascending Node of the target plane (degrees).</param>
/// <param name="inc">Inclination of the target plane (degrees).</param>
///
public static double TimeToPlane(double rotationPeriod, double latitude, double celestialLongitude, double LAN, double inc)
{
// alpha is the 90 degree angle between the line of longitude and the equator and omitted
double beta = OrbitalManeuverCalculator.HeadingForInclination(inc, latitude) * UtilMath.Deg2Rad;
double c = Math.Abs(latitude) * UtilMath.Deg2Rad; // Abs for south hemisphere launch sites
// b is how many radians to the west of the launch site that the LAN is (east in south hemisphere)
double b = Math.Atan2(2 * Math.Sin(beta), Math.Cos(beta) / Math.Tan(c / 2) + Math.Tan(c / 2) * Math.Cos(beta)); // napier's analogies
// LAN if we launched now
double LANnow = celestialLongitude - Math.Sign(latitude) * b * UtilMath.Rad2Deg;
return(MuUtils.ClampDegrees360(LAN - LANnow) / 360 * rotationPeriod);
}
// Compute an angular heading from point a to point b on a unit sphere
public static double Heading(double lat_a, double long_a, double lat_b, double long_b)
{
// Using Great-Circle Navigation formula for initial heading from http://en.wikipedia.org/wiki/Great-circle_navigation
// Note the switch from degrees to radians and back
// Original equation returns 0 for due south, increasing clockwise. We add 180 and clamp to 0-360 degrees to map to compass-type headings
double lat_a_rad = Math.PI / 180 * lat_a;
double lat_b_rad = Math.PI / 180 * lat_b;
double long_diff_rad = Math.PI / 180 * (long_b - long_a);
return(MuUtils.ClampDegrees360(180.0 / Math.PI * Math.Atan2(
Math.Sin(long_diff_rad),
Math.Cos(lat_a_rad) * Math.Tan(lat_b_rad) - Math.Sin(lat_a_rad) * Math.Cos(long_diff_rad))));
}
//Computes the deltaV of the burn needed to set a given LAN at a given UT.
public static Vector3d DeltaVToShiftLAN(Orbit o, double UT, double newLAN)
{
Vector3d pos = o.SwappedAbsolutePositionAtUT(UT);
// Burn position in the same reference frame as LAN
double burn_latitude = o.referenceBody.GetLatitude(pos);
double burn_longitude = o.referenceBody.GetLongitude(pos) + o.referenceBody.rotationAngle;
const double target_latitude = 0; // Equator
double target_longitude = 0; // Prime Meridian
// Select the location of either the descending or ascending node.
// If the descending node is closer than the ascending node, or there is no ascending node, target the reverse of the newLAN
// Otherwise target the newLAN
if (o.AscendingNodeEquatorialExists() && o.DescendingNodeEquatorialExists())
{
if (o.TimeOfDescendingNodeEquatorial(UT) < o.TimeOfAscendingNodeEquatorial(UT))
{
// DN is closer than AN
// Burning for the AN would entail flipping the orbit around, and would be very expensive
// therefore, burn for the corresponding Longitude of the Descending Node
target_longitude = MuUtils.ClampDegrees360(newLAN + 180.0);
}
else
{
// DN is closer than AN
target_longitude = MuUtils.ClampDegrees360(newLAN);
}
}
else if (o.AscendingNodeEquatorialExists() && !o.DescendingNodeEquatorialExists())
{
// No DN
target_longitude = MuUtils.ClampDegrees360(newLAN);
}
else if (!o.AscendingNodeEquatorialExists() && o.DescendingNodeEquatorialExists())
{
// No AN
target_longitude = MuUtils.ClampDegrees360(newLAN + 180.0);
}
else
{
throw new ArgumentException("OrbitalManeuverCalculator.DeltaVToShiftLAN: No Equatorial Nodes");
}
double desiredHeading = MuUtils.ClampDegrees360(Heading(burn_latitude, burn_longitude, target_latitude, target_longitude));
Vector3d actualHorizontalVelocity = Vector3d.Exclude(o.Up(UT), o.SwappedOrbitalVelocityAtUT(UT));
Vector3d eastComponent = actualHorizontalVelocity.magnitude * Math.Sin(Math.PI / 180 * desiredHeading) * o.East(UT);
Vector3d northComponent = actualHorizontalVelocity.magnitude * Math.Cos(Math.PI / 180 * desiredHeading) * o.North(UT);
Vector3d desiredHorizontalVelocity = eastComponent + northComponent;
return(desiredHorizontalVelocity - actualHorizontalVelocity);
}
public override void OnFixedUpdate()
{
if (markUT == 0)
{
Mark();
}
timeSinceMark = vesselState.time - markUT;
if (vessel.situation == Vessel.Situations.PRELAUNCH)
{
Mark(); //keep resetting stats until we launch
return;
}
gravityLosses += vesselState.deltaT * Vector3d.Dot(-vesselState.surfaceVelocity.normalized, vesselState.gravityForce);
gravityLosses -= vesselState.deltaT * Vector3d.Dot(vesselState.surfaceVelocity.normalized, vesselState.up * vesselState.radius * Math.Pow(2 * Math.PI / part.vessel.mainBody.rotationPeriod, 2));
dragLosses += vesselState.deltaT * vesselState.drag;
maxDragGees = Math.Max(maxDragGees, vesselState.drag / 9.81);
double circularPeriod = 2 * Math.PI * vesselState.radius / OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, vesselState.radius);
double angleTraversed = (vesselState.longitude - markLongitude) + 360 * (vesselState.time - markUT) / part.vessel.mainBody.rotationPeriod;
phaseAngleFromMark = MuUtils.ClampDegrees360(360 * (vesselState.time - markUT) / circularPeriod - angleTraversed);
if (paused)
{
return;
}
//int oldHistoryIdx = historyIdx;
//historyIdx = Mathf.Min(Mathf.FloorToInt((float)(timeSinceMark / precision)), history.Length - 1);
if (vesselState.time >= (lastRecordTime + precision) && historyIdx < history.Length - 1)
{
lastRecordTime = vesselState.time;
historyIdx++;
Record(historyIdx);
//if (TimeWarp.WarpMode == TimeWarp.Modes.HIGH)
// print("WRP " + historyIdx + " " + history[historyIdx].downRange.ToString("F0") + " " + history[historyIdx].AoA.ToString("F2"));
//else
//{
// print("STD " + historyIdx + " " + history[historyIdx].downRange.ToString("F0") + " " + history[historyIdx].AoA.ToString("F2"));
//}
}
}
// projects the two vectors onto the normal plane and computes the 0 to 360 angle
public static double AngleInPlane(this Vector3d vector, Vector3d planeNormal, Vector3d other)
{
Vector3d v1 = vector.ProjectOnPlane(planeNormal);
Vector3d v2 = other.ProjectOnPlane(planeNormal);
if ((v1.magnitude == 0) || (v2.magnitude == 0))
{
return(double.NaN);
}
double angle = MuUtils.ClampDegrees360(Math.Acos(Vector3d.Dot(v1.normalized, v2.normalized)) * UtilMath.Rad2Deg);
if (Vector3d.Dot(Vector3d.Cross(v1, v2), planeNormal) < 0)
{
return(-angle);
}
else
{
return(angle);
}
}
// 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);
}
}
//Gives the true anomaly at which o crosses the equator going southwards, if o is east-moving,
//or northwards, if o is west-moving.
//The returned value is always between 0 and 360.
public static double DescendingNodeEquatorialTrueAnomaly(this Orbit o)
{
return(MuUtils.ClampDegrees360(o.AscendingNodeEquatorialTrueAnomaly() + 180));
}
//Gives the true anomaly (in a's orbit) at which a crosses its descending node
//with b's orbit.
//The returned value is always between 0 and 360.
public static double DescendingNodeTrueAnomaly(this Orbit a, Orbit b)
{
return(MuUtils.ClampDegrees360(a.AscendingNodeTrueAnomaly(b) + 180));
}
public double HeadingFromDirection(Vector3d dir)
{
return(MuUtils.ClampDegrees360(180 / Math.PI * Math.Atan2(Vector3d.Dot(dir, east), Vector3d.Dot(dir, north))));
}
public void Update(Vessel vessel)
{
if (vessel.rigidbody == null)
{
return; //if we try to update before rigidbodies exist we spam the console with NullPointerExceptions.
}
//if (vessel.packed) return;
time = Planetarium.GetUniversalTime();
deltaT = TimeWarp.fixedDeltaTime;
CoM = vessel.findWorldCenterOfMass();
up = (CoM - vessel.mainBody.position).normalized;
Rigidbody rigidBody = vessel.rootPart.rigidbody;
if (rigidBody != null)
{
rootPartPos = rigidBody.position;
}
north = Vector3d.Exclude(up, (vessel.mainBody.position + vessel.mainBody.transform.up * (float)vessel.mainBody.Radius) - CoM).normalized;
east = vessel.mainBody.getRFrmVel(CoM).normalized;
forward = vessel.GetTransform().up;
rotationSurface = Quaternion.LookRotation(north, up);
rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * rotationSurface);
velocityVesselOrbit = vessel.orbit.GetVel();
velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
velocityVesselSurfaceUnit = velocityVesselSurface.normalized;
velocityMainBodySurface = rotationSurface * velocityVesselSurface;
horizontalOrbit = Vector3d.Exclude(up, velocityVesselOrbit).normalized;
horizontalSurface = Vector3d.Exclude(up, velocityVesselSurface).normalized;
angularVelocity = Quaternion.Inverse(vessel.GetTransform().rotation) * vessel.rigidbody.angularVelocity;
radialPlusSurface = Vector3d.Exclude(velocityVesselSurface, up).normalized;
radialPlus = Vector3d.Exclude(velocityVesselOrbit, up).normalized;
normalPlusSurface = -Vector3d.Cross(radialPlusSurface, velocityVesselSurfaceUnit);
normalPlus = -Vector3d.Cross(radialPlus, velocityVesselOrbitUnit);
gravityForce = FlightGlobals.getGeeForceAtPosition(CoM);
localg = gravityForce.magnitude;
speedOrbital.value = velocityVesselOrbit.magnitude;
speedSurface.value = velocityVesselSurface.magnitude;
speedVertical.value = Vector3d.Dot(velocityVesselSurface, up);
speedSurfaceHorizontal.value = (velocityVesselSurface - (speedVertical * up)).magnitude;
speedOrbitHorizontal = (velocityVesselOrbit - (speedVertical * up)).magnitude;
vesselHeading.value = rotationVesselSurface.eulerAngles.y;
vesselPitch.value = (rotationVesselSurface.eulerAngles.x > 180) ? (360.0 - rotationVesselSurface.eulerAngles.x) : -rotationVesselSurface.eulerAngles.x;
vesselRoll.value = (rotationVesselSurface.eulerAngles.z > 180) ? (rotationVesselSurface.eulerAngles.z - 360.0) : rotationVesselSurface.eulerAngles.z;
altitudeASL.value = vessel.mainBody.GetAltitude(CoM);
RaycastHit sfc;
if (Physics.Raycast(CoM, -up, out sfc, (float)altitudeASL + 10000.0F, 1 << 15))
{
altitudeTrue.value = sfc.distance;
}
else if (vessel.mainBody.pqsController != null)
{
// from here: http://kerbalspaceprogram.com/forum/index.php?topic=10324.msg161923#msg161923
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM) - (vessel.mainBody.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(vessel.mainBody.GetLongitude(CoM), Vector3d.down) * QuaternionD.AngleAxis(vessel.mainBody.GetLatitude(CoM), Vector3d.forward) * Vector3d.right) - vessel.mainBody.pqsController.radius);
}
else
{
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM);
}
double surfaceAltitudeASL = altitudeASL - altitudeTrue;
altitudeBottom = altitudeTrue;
foreach (Part p in vessel.parts)
{
if (p.collider != null)
{
Vector3d bottomPoint = p.collider.ClosestPointOnBounds(vessel.mainBody.position);
double partBottomAlt = vessel.mainBody.GetAltitude(bottomPoint) - surfaceAltitudeASL;
altitudeBottom = Math.Max(0, Math.Min(altitudeBottom, partBottomAlt));
}
}
double atmosphericPressure = FlightGlobals.getStaticPressure(altitudeASL, vessel.mainBody);
if (atmosphericPressure < vessel.mainBody.atmosphereMultiplier * 1e-6)
{
atmosphericPressure = 0;
}
atmosphericDensity = FlightGlobals.getAtmDensity(atmosphericPressure);
atmosphericDensityGrams = atmosphericDensity * 1000;
orbitApA.value = vessel.orbit.ApA;
orbitPeA.value = vessel.orbit.PeA;
orbitPeriod.value = vessel.orbit.period;
orbitTimeToAp.value = vessel.orbit.timeToAp;
if (vessel.orbit.eccentricity < 1)
{
orbitTimeToPe.value = vessel.orbit.timeToPe;
}
else
{
orbitTimeToPe.value = -vessel.orbit.meanAnomaly / (2 * Math.PI / vessel.orbit.period);
}
orbitLAN.value = vessel.orbit.LAN;
orbitArgumentOfPeriapsis.value = vessel.orbit.argumentOfPeriapsis;
orbitInclination.value = vessel.orbit.inclination;
orbitEccentricity.value = vessel.orbit.eccentricity;
orbitSemiMajorAxis.value = vessel.orbit.semiMajorAxis;
latitude.value = vessel.mainBody.GetLatitude(CoM);
longitude.value = MuUtils.ClampDegrees180(vessel.mainBody.GetLongitude(CoM));
if (vessel.mainBody != Planetarium.fetch.Sun)
{
Vector3d delta = vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + 1) - vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() - 1);
Vector3d plUp = Vector3d.Cross(vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime()) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime()), vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4)).normalized;
angleToPrograde = MuUtils.ClampDegrees360((((vessel.orbit.inclination > 90) || (vessel.orbit.inclination < -90)) ? 1 : -1) * ((Vector3)up).AngleInPlane(plUp, delta));
}
else
{
angleToPrograde = 0;
}
mainBody = vessel.mainBody;
radius = (CoM - vessel.mainBody.position).magnitude;
mass = thrustAvailable = thrustMinimum = massDrag = torqueRAvailable = torquePYAvailable = torqueThrustPYAvailable = 0;
rcsThrustAvailable = new Vector6();
rcsTorqueAvailable = new Vector6();
EngineInfo einfo = new EngineInfo(forward, CoM);
IntakeInfo iinfo = new IntakeInfo();
var rcsbal = vessel.GetMasterMechJeb().rcsbal;
if (vessel.ActionGroups[KSPActionGroup.RCS] && rcsbal.enabled)
{
Vector3d rot = Vector3d.zero;
foreach (Vector6.Direction dir6 in Enum.GetValues(typeof(Vector6.Direction)))
{
Vector3d dir = Vector6.directions[dir6];
double[] throttles;
List <RCSSolver.Thruster> thrusters;
rcsbal.GetThrottles(dir, out throttles, out thrusters);
if (throttles != null)
{
for (int i = 0; i < throttles.Length; i++)
{
if (throttles[i] > 0)
{
Vector3d force = thrusters[i].GetThrust(dir, rot);
rcsThrustAvailable.Add(dir * Vector3d.Dot(force * throttles[i], dir));
}
}
}
}
}
foreach (Part p in vessel.parts)
{
if (p.physicalSignificance != Part.PhysicalSignificance.NONE)
{
double partMass = p.TotalMass();
mass += partMass;
massDrag += partMass * p.maximum_drag;
}
if (vessel.ActionGroups[KSPActionGroup.RCS] && !rcsbal.enabled)
{
foreach (ModuleRCS pm in p.Modules.OfType <ModuleRCS>())
{
double maxT = pm.thrusterPower;
if ((pm.isEnabled) && (!pm.isJustForShow))
{
torqueRAvailable += maxT;
if (p.Rigidbody != null)
{
torquePYAvailable += maxT * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
}
foreach (Transform t in pm.thrusterTransforms)
{
rcsThrustAvailable.Add(-t.up * pm.thrusterPower);
}
}
}
}
if (p is CommandPod)
{
torqueRAvailable += Math.Abs(((CommandPod)p).rotPower);
torquePYAvailable += Math.Abs(((CommandPod)p).rotPower);
}
foreach (PartModule pm in p.Modules)
{
if (!pm.isEnabled)
{
continue;
}
if (pm is ModuleEngines)
{
einfo.AddNewEngine(pm as ModuleEngines);
}
else if (pm is ModuleResourceIntake)
{
iinfo.addIntake(pm as ModuleResourceIntake);
}
}
}
thrustAvailable += einfo.thrustAvailable;
thrustMinimum += einfo.thrustMinimum;
torqueThrustPYAvailable += einfo.torqueThrustPYAvailable;
// Convert the resource information from the einfo and iinfo format
// to the more useful ResourceInfo format.
resources = new Dictionary <int, ResourceInfo>();
foreach (var info in einfo.resourceRequired)
{
int id = info.Key;
var req = info.Value;
resources[id] = new ResourceInfo(
PartResourceLibrary.Instance.GetDefinition(id),
req.requiredLastFrame,
req.requiredAtMaxThrottle,
iinfo.getIntakes(id));
}
int intakeAirId = PartResourceLibrary.Instance.GetDefinition("IntakeAir").id;
intakeAir = 0;
intakeAirNeeded = 0;
intakeAirAtMax = 0;
if (resources.ContainsKey(intakeAirId))
{
intakeAir = resources[intakeAirId].intakeProvided;
intakeAirNeeded = resources[intakeAirId].required;
intakeAirAtMax = resources[intakeAirId].requiredAtMaxThrottle;
}
angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
maxThrustAccel = thrustAvailable / mass;
minThrustAccel = thrustMinimum / mass;
inertiaTensor = new Matrix3x3();
foreach (Part p in vessel.parts)
{
if (p.Rigidbody == null)
{
continue;
}
//Compute the contributions to the vessel inertia tensor due to the part inertia tensor
Vector3d principalMoments = p.Rigidbody.inertiaTensor;
Quaternion princAxesRot = Quaternion.Inverse(vessel.GetTransform().rotation) * p.transform.rotation * p.Rigidbody.inertiaTensorRotation;
Quaternion invPrincAxesRot = Quaternion.Inverse(princAxesRot);
for (int i = 0; i < 3; i++)
{
Vector3d iHat = Vector3d.zero;
iHat[i] = 1;
for (int j = 0; j < 3; j++)
{
Vector3d jHat = Vector3d.zero;
jHat[j] = 1;
inertiaTensor[i, j] += Vector3d.Dot(iHat, princAxesRot * Vector3d.Scale(principalMoments, invPrincAxesRot * jHat));
}
}
//Compute the contributions to the vessel inertia tensor due to the part mass and position
double partMass = p.TotalMass();
Vector3 partPosition = vessel.transform.InverseTransformDirection(p.Rigidbody.worldCenterOfMass - CoM);
for (int i = 0; i < 3; i++)
{
inertiaTensor[i, i] += partMass * partPosition.sqrMagnitude;
for (int j = 0; j < 3; j++)
{
inertiaTensor[i, j] += -partMass * partPosition[i] * partPosition[j];
}
}
}
MoI = new Vector3d(inertiaTensor[0, 0], inertiaTensor[1, 1], inertiaTensor[2, 2]);
angularMomentum = inertiaTensor * angularVelocity;
}
public void Update(Vessel vessel)
{
if (vessel.rigidbody == null)
{
return; //if we try to update before rigidbodies exist we spam the console with NullPointerExceptions.
}
//if (vessel.packed) return;
// To investigate some strange error
if ((vessel.mainBody == null || (object)(vessel.mainBody) == null) && counter == 0)
{
if ((object)(vessel.mainBody) == null)
{
MechJebCore.print("vessel.mainBody is proper null");
}
else
{
MechJebCore.print("vessel.mainBody is Unity null");
}
counter = counter++ % 100;
}
time = Planetarium.GetUniversalTime();
deltaT = TimeWarp.fixedDeltaTime;
CoM = vessel.findWorldCenterOfMass();
up = (CoM - vessel.mainBody.position).normalized;
Rigidbody rigidBody = vessel.rootPart.rigidbody;
if (rigidBody != null)
{
rootPartPos = rigidBody.position;
}
north = Vector3d.Exclude(up, (vessel.mainBody.position + vessel.mainBody.transform.up * (float)vessel.mainBody.Radius) - CoM).normalized;
east = vessel.mainBody.getRFrmVel(CoM).normalized;
forward = vessel.GetTransform().up;
rotationSurface = Quaternion.LookRotation(north, up);
rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * rotationSurface);
// velocityVesselOrbit = vessel.orbit.GetVel();
// velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
// velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
// velocityVesselSurfaceUnit = velocityVesselSurface.normalized;
velocityMainBodySurface = rotationSurface * vessel.srf_velocity;
horizontalOrbit = Vector3d.Exclude(up, vessel.obt_velocity).normalized;
horizontalSurface = Vector3d.Exclude(up, vessel.srf_velocity).normalized;
angularVelocity = Quaternion.Inverse(vessel.GetTransform().rotation) * vessel.rigidbody.angularVelocity;
radialPlusSurface = Vector3d.Exclude(vessel.srf_velocity, up).normalized;
radialPlus = Vector3d.Exclude(vessel.obt_velocity, up).normalized;
normalPlusSurface = -Vector3d.Cross(radialPlusSurface, vessel.srf_velocity.normalized);
normalPlus = -Vector3d.Cross(radialPlus, vessel.obt_velocity.normalized);
gravityForce = FlightGlobals.getGeeForceAtPosition(CoM);
localg = gravityForce.magnitude;
speedOrbital.value = vessel.obt_velocity.magnitude;
speedSurface.value = vessel.srf_velocity.magnitude;
speedVertical.value = Vector3d.Dot(vessel.srf_velocity, up);
speedSurfaceHorizontal.value = Vector3d.Exclude(up, vessel.srf_velocity).magnitude; //(velocityVesselSurface - (speedVertical * up)).magnitude;
speedOrbitHorizontal = (vessel.obt_velocity - (speedVertical * up)).magnitude;
vesselHeading.value = rotationVesselSurface.eulerAngles.y;
vesselPitch.value = (rotationVesselSurface.eulerAngles.x > 180) ? (360.0 - rotationVesselSurface.eulerAngles.x) : -rotationVesselSurface.eulerAngles.x;
vesselRoll.value = (rotationVesselSurface.eulerAngles.z > 180) ? (rotationVesselSurface.eulerAngles.z - 360.0) : rotationVesselSurface.eulerAngles.z;
altitudeASL.value = vessel.mainBody.GetAltitude(CoM);
//RaycastHit sfc;
//if (Physics.Raycast(CoM, -up, out sfc, (float)altitudeASL + 10000.0F, 1 << 15))
//{
// altitudeTrue.value = sfc.distance;
//}
//else if (vessel.mainBody.pqsController != null)
//{
// // from here: http://kerbalspaceprogram.com/forum/index.php?topic=10324.msg161923#msg161923
// altitudeTrue.value = vessel.mainBody.GetAltitude(CoM) - (vessel.mainBody.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(vessel.mainBody.GetLongitude(CoM), Vector3d.down) * QuaternionD.AngleAxis(vessel.mainBody.GetLatitude(CoM), Vector3d.forward) * Vector3d.right) - vessel.mainBody.pqsController.radius);
//}
//else
//{
// altitudeTrue.value = vessel.mainBody.GetAltitude(CoM);
//}
//double surfaceAltitudeASL = altitudeASL - altitudeTrue;
double surfaceAltitudeASL = vessel.mainBody.pqsController != null ? vessel.pqsAltitude : 0d;
altitudeTrue.value = altitudeASL - surfaceAltitudeASL;
altitudeBottom = altitudeTrue;
foreach (Part p in vessel.parts)
{
if (p.collider != null)
{
Vector3d bottomPoint = p.collider.ClosestPointOnBounds(vessel.mainBody.position);
double partBottomAlt = vessel.mainBody.GetAltitude(bottomPoint) - surfaceAltitudeASL;
altitudeBottom = Math.Max(0, Math.Min(altitudeBottom, partBottomAlt));
}
}
double atmosphericPressure = FlightGlobals.getStaticPressure(altitudeASL, vessel.mainBody);
if (atmosphericPressure < vessel.mainBody.atmosphereMultiplier * 1e-6)
{
atmosphericPressure = 0;
}
atmosphericDensity = FlightGlobals.getAtmDensity(atmosphericPressure);
atmosphericDensityGrams = atmosphericDensity * 1000;
orbitApA.value = vessel.orbit.ApA;
orbitPeA.value = vessel.orbit.PeA;
orbitPeriod.value = vessel.orbit.period;
orbitTimeToAp.value = vessel.orbit.timeToAp;
if (vessel.orbit.eccentricity < 1)
{
orbitTimeToPe.value = vessel.orbit.timeToPe;
}
else
{
orbitTimeToPe.value = -vessel.orbit.meanAnomaly / (2 * Math.PI / vessel.orbit.period);
}
orbitLAN.value = vessel.orbit.LAN;
orbitArgumentOfPeriapsis.value = vessel.orbit.argumentOfPeriapsis;
orbitInclination.value = vessel.orbit.inclination;
orbitEccentricity.value = vessel.orbit.eccentricity;
orbitSemiMajorAxis.value = vessel.orbit.semiMajorAxis;
latitude.value = vessel.mainBody.GetLatitude(CoM);
longitude.value = MuUtils.ClampDegrees180(vessel.mainBody.GetLongitude(CoM));
if (vessel.mainBody != Planetarium.fetch.Sun)
{
Vector3d delta = vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + 1) - vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() - 1);
Vector3d plUp = Vector3d.Cross(vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime()) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime()), vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4)).normalized;
angleToPrograde = MuUtils.ClampDegrees360((((vessel.orbit.inclination > 90) || (vessel.orbit.inclination < -90)) ? 1 : -1) * ((Vector3)up).AngleInPlane(plUp, delta));
}
else
{
angleToPrograde = 0;
}
mainBody = vessel.mainBody;
radius = (CoM - vessel.mainBody.position).magnitude;
mass = massDrag = torqueThrustPYAvailable = 0;
thrustVectorLastFrame = new Vector3d();
thrustVectorMaxThrottle = new Vector3d();
thrustVectorMinThrottle = new Vector3d();
torqueAvailable = new Vector3d();
rcsThrustAvailable = new Vector6();
rcsTorqueAvailable = new Vector6();
ctrlTorqueAvailable = new Vector6();
EngineInfo einfo = new EngineInfo(CoM);
IntakeInfo iinfo = new IntakeInfo();
parachutes = new List <ModuleParachute>();
var rcsbal = vessel.GetMasterMechJeb().rcsbal;
if (vessel.ActionGroups[KSPActionGroup.RCS] && rcsbal.enabled)
{
Vector3d rot = Vector3d.zero;
foreach (Vector6.Direction dir6 in Enum.GetValues(typeof(Vector6.Direction)))
{
Vector3d dir = Vector6.directions[dir6];
double[] throttles;
List <RCSSolver.Thruster> thrusters;
rcsbal.GetThrottles(dir, out throttles, out thrusters);
if (throttles != null)
{
for (int i = 0; i < throttles.Length; i++)
{
if (throttles[i] > 0)
{
Vector3d force = thrusters[i].GetThrust(dir, rot);
rcsThrustAvailable.Add(vessel.GetTransform().InverseTransformDirection(dir * Vector3d.Dot(force * throttles[i], dir)));
}
}
}
}
}
hasMFE = false;
foreach (Part p in vessel.parts)
{
if (p.IsPhysicallySignificant())
{
double partMass = p.TotalMass();
mass += partMass;
massDrag += partMass * p.maximum_drag;
}
if (vessel.ActionGroups[KSPActionGroup.RCS] && !rcsbal.enabled)
{
foreach (ModuleRCS pm in p.Modules.OfType <ModuleRCS>())
{
double maxT = pm.thrusterPower;
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
if ((pm.isEnabled) && (!pm.isJustForShow))
{
foreach (Transform t in pm.thrusterTransforms)
{
Vector3d thrusterThrust = vessel.GetTransform().InverseTransformDirection(-t.up.normalized) * pm.thrusterPower;
rcsThrustAvailable.Add(thrusterThrust);
Vector3d thrusterTorque = Vector3.Cross(vessel.GetTransform().InverseTransformDirection(partPosition), thrusterThrust);
rcsTorqueAvailable.Add(thrusterTorque);
}
}
}
}
if (p is ControlSurface)
{
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
ControlSurface cs = (p as ControlSurface);
Vector3d airSpeed = vessel.srf_velocity + Vector3.Cross(cs.Rigidbody.angularVelocity, cs.transform.position - cs.Rigidbody.position);
// Air Speed is velocityVesselSurface
// AddForceAtPosition seems to need the airspeed vector rotated with the flap rotation x its surface
Quaternion airSpeedRot = Quaternion.AngleAxis(cs.ctrlSurfaceRange * cs.ctrlSurfaceArea, cs.transform.rotation * cs.pivotAxis);
Vector3 ctrlTroquePos = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector(airSpeedRot * airSpeed)));
Vector3 ctrlTroqueNeg = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector(Quaternion.Inverse(airSpeedRot) * airSpeed)));
ctrlTorqueAvailable.Add(ctrlTroquePos);
ctrlTorqueAvailable.Add(ctrlTroqueNeg);
}
if (p is CommandPod)
{
torqueAvailable += Vector3d.one * Math.Abs(((CommandPod)p).rotPower);
}
foreach (VesselStatePartExtension vspe in vesselStatePartExtensions)
{
vspe(p);
}
foreach (PartModule pm in p.Modules)
{
if (!pm.isEnabled)
{
continue;
}
if (pm is ModuleReactionWheel)
{
ModuleReactionWheel rw = (ModuleReactionWheel)pm;
// I had to remove the test for active in .23 since the new ressource system reply to the RW that
// there is no energy available when the RW do tiny adjustement.
// I replaceed it with a test that check if there is electricity anywhere on the ship.
// Let's hope we don't get reaction wheel that use something else
//if (rw.wheelState == ModuleReactionWheel.WheelState.Active && !rw.stateString.Contains("Not enough"))
if (rw.wheelState == ModuleReactionWheel.WheelState.Active && vessel.HasElectricCharge())
{
torqueAvailable += new Vector3d(rw.PitchTorque, rw.RollTorque, rw.YawTorque);
}
}
else if (pm is ModuleEngines)
{
einfo.AddNewEngine(pm as ModuleEngines);
}
else if (pm is ModuleEnginesFX)
{
einfo.AddNewEngine(pm as ModuleEnginesFX);
}
else if (pm is ModuleResourceIntake)
{
iinfo.addIntake(pm as ModuleResourceIntake);
}
else if (pm is ModuleParachute)
{
parachutes.Add(pm as ModuleParachute);
}
else if (pm is ModuleControlSurface)
{
// TODO : Tweakable for ignorePitch / ignoreYaw / ignoreRoll
ModuleControlSurface cs = (pm as ModuleControlSurface);
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
Vector3d airSpeed = vessel.srf_velocity + Vector3.Cross(cs.part.Rigidbody.angularVelocity, cs.transform.position - cs.part.Rigidbody.position);
Quaternion airSpeedRot = Quaternion.AngleAxis(cs.ctrlSurfaceRange * cs.ctrlSurfaceArea, cs.transform.rotation * Vector3.right);
Vector3 ctrlTroquePos = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector(airSpeedRot * airSpeed)));
Vector3 ctrlTroqueNeg = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector(Quaternion.Inverse(airSpeedRot) * airSpeed)));
ctrlTorqueAvailable.Add(ctrlTroquePos);
ctrlTorqueAvailable.Add(ctrlTroqueNeg);
}
if (pm.ClassName == "ModuleEngineConfigs" || pm.ClassName == "ModuleHybridEngine" || pm.ClassName == "ModuleHybridEngines")
{
hasMFE = true;
}
foreach (VesselStatePartModuleExtension vspme in vesselStatePartModuleExtensions)
{
vspme(pm);
}
}
}
// Consider all the parachutes
{
bool tempParachuteDeployed = false;
foreach (ModuleParachute p in parachutes)
{
if (p.deploymentState == ModuleParachute.deploymentStates.DEPLOYED || p.deploymentState == ModuleParachute.deploymentStates.SEMIDEPLOYED)
{
tempParachuteDeployed = true;
break;
}
}
this.parachuteDeployed = tempParachuteDeployed;
}
torqueAvailable += Vector3d.Max(rcsTorqueAvailable.positive, rcsTorqueAvailable.negative); // Should we use Max or Min ?
torqueAvailable += Vector3d.Max(ctrlTorqueAvailable.positive, ctrlTorqueAvailable.negative); // Should we use Max or Min ?
thrustVectorMaxThrottle += einfo.thrustMax;
thrustVectorMinThrottle += einfo.thrustMin;
thrustVectorLastFrame += einfo.thrustCurrent;
torqueThrustPYAvailable += einfo.torqueThrustPYAvailable;
if (thrustVectorMaxThrottle.magnitude == 0 && vessel.ActionGroups[KSPActionGroup.RCS])
{
rcsThrust = true;
thrustVectorMaxThrottle += (Vector3d)(vessel.transform.up) * rcsThrustAvailable.down;
}
else
{
rcsThrust = false;
}
// Convert the resource information from the einfo and iinfo format
// to the more useful ResourceInfo format.
resources = new Dictionary <int, ResourceInfo>();
foreach (var info in einfo.resourceRequired)
{
int id = info.Key;
var req = info.Value;
resources[id] = new ResourceInfo(
PartResourceLibrary.Instance.GetDefinition(id),
req.requiredLastFrame,
req.requiredAtMaxThrottle,
iinfo.getIntakes(id));
}
int intakeAirId = PartResourceLibrary.Instance.GetDefinition("IntakeAir").id;
intakeAir = 0;
intakeAirNeeded = 0;
intakeAirAtMax = 0;
intakeAirAllIntakes = 0;
if (resources.ContainsKey(intakeAirId))
{
intakeAir = resources[intakeAirId].intakeProvided;
intakeAirAllIntakes = resources[intakeAirId].intakeAvailable;
intakeAirNeeded = resources[intakeAirId].required;
intakeAirAtMax = resources[intakeAirId].requiredAtMaxThrottle;
}
angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
inertiaTensor = new Matrix3x3();
foreach (Part p in vessel.parts)
{
if (p.Rigidbody == null)
{
continue;
}
//Compute the contributions to the vessel inertia tensor due to the part inertia tensor
Vector3d principalMoments = p.Rigidbody.inertiaTensor;
Quaternion princAxesRot = Quaternion.Inverse(vessel.GetTransform().rotation) * p.transform.rotation * p.Rigidbody.inertiaTensorRotation;
Quaternion invPrincAxesRot = Quaternion.Inverse(princAxesRot);
for (int i = 0; i < 3; i++)
{
Vector3d iHat = Vector3d.zero;
iHat[i] = 1;
for (int j = 0; j < 3; j++)
{
Vector3d jHat = Vector3d.zero;
jHat[j] = 1;
inertiaTensor[i, j] += Vector3d.Dot(iHat, princAxesRot * Vector3d.Scale(principalMoments, invPrincAxesRot * jHat));
}
}
//Compute the contributions to the vessel inertia tensor due to the part mass and position
double partMass = p.TotalMass();
Vector3 partPosition = vessel.GetTransform().InverseTransformDirection(p.Rigidbody.worldCenterOfMass - CoM);
for (int i = 0; i < 3; i++)
{
inertiaTensor[i, i] += partMass * partPosition.sqrMagnitude;
for (int j = 0; j < 3; j++)
{
inertiaTensor[i, j] += -partMass * partPosition[i] * partPosition[j];
}
}
}
MoI = new Vector3d(inertiaTensor[0, 0], inertiaTensor[1, 1], inertiaTensor[2, 2]);
angularMomentum = inertiaTensor * angularVelocity;
}
protected override void WindowGUI(int windowID)
{
if (btNormal == null)
{
btNormal = new GUIStyle(GUI.skin.button);
btNormal.normal.textColor = btNormal.focused.textColor = Color.white;
btNormal.hover.textColor = btNormal.active.textColor = Color.yellow;
btNormal.onNormal.textColor = btNormal.onFocused.textColor = btNormal.onHover.textColor = btNormal.onActive.textColor = Color.green;
//btNormal.padding = new RectOffset(8, 8, 8, 8);
btActive = new GUIStyle(btNormal);
btActive.active = btActive.onActive;
btActive.normal = btActive.onNormal;
btActive.onFocused = btActive.focused;
btActive.hover = btActive.onHover;
btGreen = new GUIStyle(btNormal);
btGreen.normal.textColor = Color.green;
btGreen.fixedWidth = 35;
btWhite = new GUIStyle(btNormal);
btWhite.normal.textColor = Color.white;
btWhite.fixedWidth = 35;
btAuto = new GUIStyle(btNormal);
btAuto.padding = new RectOffset(8, 8, 8, 8);
btAuto.normal.textColor = Color.red;
btAuto.onActive = btAuto.onFocused = btAuto.onHover = btAuto.onNormal = btAuto.active = btAuto.focused = btAuto.hover = btAuto.normal;
}
if (autopilot.enabled)
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_button1"), btActive)) //Disengage autopilot
{
autopilot.users.Remove(this);
}
}
else if (core.attitude.enabled && core.attitude.users.Count(u => !this.Equals(u)) > 0)
{
if (core.attitude.users.Contains(this))
{
core.attitude.users.Remove(this); // so we don't suddenly turn on when the other autopilot finishes
}
GUILayout.Button("Auto", btAuto, GUILayout.ExpandWidth(true));
}
else
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_button2"))) //Engage autopilot
{
autopilot.users.Add(this);
}
}
GUILayout.BeginHorizontal();
bool AltitudeHold = autopilot.AltitudeHoldEnabled;
autopilot.AltitudeHoldEnabled = GUILayout.Toggle(autopilot.AltitudeHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label1"), GUILayout.Width(140)); //Altitude Hold
if (AltitudeHold != autopilot.AltitudeHoldEnabled)
{
if (autopilot.AltitudeHoldEnabled)
{
autopilot.EnableAltitudeHold();
}
else
{
autopilot.DisableAltitudeHold();
}
}
bool change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
AltitudeTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
AltitudeTargettmp.text = GUILayout.TextField(AltitudeTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
AltitudeTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (AltitudeTargettmp < 0)
{
AltitudeTargettmp = 0;
}
GUILayout.Label("m", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset1"), autopilot.AltitudeTarget == AltitudeTargettmp ? btWhite : btGreen)) //Set
{
autopilot.AltitudeTarget = AltitudeTargettmp;
}
GUILayout.EndHorizontal();
if (!autopilot.AltitudeHoldEnabled)
{
bool _VertSpeedHoldEnabled = autopilot.VertSpeedHoldEnabled;
GUILayout.BeginHorizontal();
autopilot.VertSpeedHoldEnabled = GUILayout.Toggle(autopilot.VertSpeedHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label2"), GUILayout.Width(140)); //Vertical Speed Hold
if (_VertSpeedHoldEnabled != autopilot.VertSpeedHoldEnabled)
{
if (autopilot.VertSpeedHoldEnabled)
{
autopilot.EnableVertSpeedHold();
}
else
{
autopilot.DisableVertSpeedHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
VertSpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp.text = GUILayout.TextField(VertSpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
VertSpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp = Math.Max(0, VertSpeedTargettmp);
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset2"), autopilot.VertSpeedTarget == VertSpeedTargettmp ? btWhite : btGreen))
{
autopilot.VertSpeedTarget = VertSpeedTargettmp;
}
GUILayout.EndHorizontal();
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_VS"), GUILayout.Width(140));//" V/S ±"
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
VertSpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp.text = GUILayout.TextField(VertSpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
VertSpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp = Math.Max(0, VertSpeedTargettmp);
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset6"), autopilot.VertSpeedTarget == VertSpeedTargettmp ? btWhite : btGreen))
{
autopilot.VertSpeedTarget = VertSpeedTargettmp;
}
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
bool _HeadingHoldEnabled = autopilot.HeadingHoldEnabled;
autopilot.HeadingHoldEnabled = GUILayout.Toggle(autopilot.HeadingHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label4"), GUILayout.Width(140)); //"Heading Hold"
if (_HeadingHoldEnabled != autopilot.HeadingHoldEnabled)
{
if (autopilot.HeadingHoldEnabled)
{
autopilot.EnableHeadingHold();
}
else
{
autopilot.DisableHeadingHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
HeadingTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
HeadingTargettmp.text = GUILayout.TextField(HeadingTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
HeadingTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
HeadingTargettmp = MuUtils.ClampDegrees360(HeadingTargettmp);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset4"), autopilot.HeadingTarget == HeadingTargettmp ? btWhite : btGreen))
{
autopilot.HeadingTarget = HeadingTargettmp;
}
GUILayout.EndHorizontal();
if (!autopilot.HeadingHoldEnabled)
{
GUILayout.BeginHorizontal();
autopilot.RollHoldEnabled = GUILayout.Toggle(autopilot.RollHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label5"), GUILayout.Width(140)); //"Roll Hold"
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
RollTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollTargettmp.text = GUILayout.TextField(RollTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
RollTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollTargettmp = MuUtils.Clamp(RollTargettmp, -90, 90);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset5"), autopilot.RollTarget == RollTargettmp ? btWhite : btGreen))
{
autopilot.RollTarget = RollTargettmp;
}
GUILayout.EndHorizontal();
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Label6"), GUILayout.Width(140)); //" Roll Limit ±"
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
RollMaxtmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollMaxtmp.text = GUILayout.TextField(RollMaxtmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
RollMaxtmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollMaxtmp = MuUtils.Clamp(RollMaxtmp, -60, 60);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset6"), autopilot.BankAngle == RollMaxtmp ? btWhite : btGreen))
{
autopilot.BankAngle = RollMaxtmp;
}
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
bool _AutoThrustCtrl = autopilot.SpeedHoldEnabled;
autopilot.SpeedHoldEnabled = GUILayout.Toggle(autopilot.SpeedHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label7"), GUILayout.Width(140)); //Speed Hold
if (autopilot.SpeedHoldEnabled != _AutoThrustCtrl)
{
if (autopilot.SpeedHoldEnabled)
{
autopilot.EnableSpeedHold();
}
else
{
autopilot.DisableSpeedHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
SpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
SpeedTargettmp.text = GUILayout.TextField(SpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
SpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (SpeedTargettmp < 0)
{
SpeedTargettmp = 0;
}
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset7"), autopilot.SpeedTarget == SpeedTargettmp ? btWhite : btGreen)) //Set
{
autopilot.SpeedTarget = SpeedTargettmp;
}
GUILayout.EndHorizontal();
if (!showpid)
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_button3"), GUILayout.Width(40))) //"PID"
{
showpid = true;
}
}
else
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_button4"), GUILayout.Width(140))) //Hide PID
{
showpid = false;
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Label8"), GUILayout.ExpandWidth(true)); //Accceleration
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.AccKp.text = GUILayout.TextField(autopilot.AccKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.AccKi.text = GUILayout.TextField(autopilot.AccKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.AccKd.text = GUILayout.TextField(autopilot.AccKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.SpeedHoldEnabled)
{
GUILayout.Label(Localizer.Format("#MecgJeb_Aircraftauto_error1", autopilot.a_err.ToString("F2"), autopilot.RealAccelerationTarget.ToString("F2"), autopilot.cur_acc.ToString("F2")), GUILayout.ExpandWidth(false)); //"error:"<<1>>" Target:"<<2>> " Cur:"<<3>>
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Pitch"), GUILayout.ExpandWidth(true));//"VertSpeed"
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.PitKp.text = GUILayout.TextField(autopilot.PitKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.PitKi.text = GUILayout.TextField(autopilot.PitKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.PitKd.text = GUILayout.TextField(autopilot.PitKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.VertSpeedHoldEnabled)
{
GUILayout.Label(Localizer.Format("#MecgJeb_Aircraftauto_error2", autopilot.pitch_err.ToString("F2"), autopilot.RealPitchTarget.ToString("F2"), vesselState.currentPitch.ToString("F2"), autopilot.pitch_act.ToString("F5"), GUILayout.ExpandWidth(false)));//error:" Target:"" Cur:"
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Label10"), GUILayout.ExpandWidth(true)); //Roll
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.RolKp.text = GUILayout.TextField(autopilot.RolKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.RolKi.text = GUILayout.TextField(autopilot.RolKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.RolKd.text = GUILayout.TextField(autopilot.RolKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.RollHoldEnabled)
{
GUILayout.Label(Localizer.Format("#MecgJeb_Aircraftauto_error2", autopilot.roll_err.ToString("F2"), autopilot.RealRollTarget.ToString("F2"), (-vesselState.currentRoll).ToString("F2"), autopilot.roll_act.ToString("F5"), GUILayout.ExpandWidth(false)));//error:" Target:"" Cur:"
}
GUILayout.BeginHorizontal();
GUILayout.Label("Yaw", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.YawKp.text = GUILayout.TextField(autopilot.YawKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.YawKi.text = GUILayout.TextField(autopilot.YawKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.YawKd.text = GUILayout.TextField(autopilot.YawKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.HeadingHoldEnabled)
{
GUILayout.Label(Localizer.Format("#MecgJeb_Aircraftauto_error2", autopilot.yaw_err.ToString("F2"), autopilot.RealYawTarget.ToString("F2"), autopilot.curr_yaw.ToString("F2"), autopilot.yaw_act.ToString("F5"), GUILayout.ExpandWidth(false)));//error:" Target:"" Cur:"
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Limits"), GUILayout.ExpandWidth(false)); //Yaw Control Limit
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_PitchDownLimit"), GUILayout.ExpandWidth(false));
autopilot.PitchDownLimit.text = GUILayout.TextField(autopilot.PitchDownLimit.text, GUILayout.Width(40));
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_PitchUpLimit"), GUILayout.ExpandWidth(false));
autopilot.PitchUpLimit.text = GUILayout.TextField(autopilot.PitchUpLimit.text, GUILayout.Width(40));
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_YawLimit"), GUILayout.ExpandWidth(false));
autopilot.YawLimit.text = GUILayout.TextField(autopilot.YawLimit.text, GUILayout.Width(40));
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_RollLimit"), GUILayout.ExpandWidth(false));
autopilot.RollLimit.text = GUILayout.TextField(autopilot.RollLimit.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
}
base.WindowGUI(windowID);
}
protected override void WindowGUI(int windowID)
{
if (btNormal == null)
{
btNormal = new GUIStyle(GUI.skin.button);
btNormal.normal.textColor = btNormal.focused.textColor = Color.white;
btNormal.hover.textColor = btNormal.active.textColor = Color.yellow;
btNormal.onNormal.textColor = btNormal.onFocused.textColor = btNormal.onHover.textColor = btNormal.onActive.textColor = Color.green;
//btNormal.padding = new RectOffset(8, 8, 8, 8);
btActive = new GUIStyle(btNormal);
btActive.active = btActive.onActive;
btActive.normal = btActive.onNormal;
btActive.onFocused = btActive.focused;
btActive.hover = btActive.onHover;
btGreen = new GUIStyle(btNormal);
btGreen.normal.textColor = Color.green;
btGreen.fixedWidth = 35;
btWhite = new GUIStyle(btNormal);
btWhite.normal.textColor = Color.white;
btWhite.fixedWidth = 35;
btAuto = new GUIStyle(btNormal);
btAuto.padding = new RectOffset(8, 8, 8, 8);
btAuto.normal.textColor = Color.red;
btAuto.onActive = btAuto.onFocused = btAuto.onHover = btAuto.onNormal = btAuto.active = btAuto.focused = btAuto.hover = btAuto.normal;
}
if (autopilot.enabled)
{
if (GUILayout.Button("Disengage autopilot", btActive))
{
autopilot.users.Remove(this);
}
}
else if (core.attitude.enabled && core.attitude.users.Count(u => !this.Equals(u)) > 0)
{
if (core.attitude.users.Contains(this))
{
core.attitude.users.Remove(this); // so we don't suddenly turn on when the other autopilot finishes
}
GUILayout.Button("Auto", btAuto, GUILayout.ExpandWidth(true));
}
else
{
if (GUILayout.Button("Engage autopilot"))
{
autopilot.users.Add(this);
}
}
GUILayout.BeginHorizontal();
bool AltitudeHold = autopilot.AltitudeHoldEnabled;
autopilot.AltitudeHoldEnabled = GUILayout.Toggle(autopilot.AltitudeHoldEnabled, "Altitude Hold", GUILayout.Width(140));
if (AltitudeHold != autopilot.AltitudeHoldEnabled)
{
if (autopilot.AltitudeHoldEnabled)
{
autopilot.EnableAltitudeHold();
}
else
{
autopilot.DisableAltitudeHold();
}
}
bool change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
AltitudeTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
AltitudeTargettmp.text = GUILayout.TextField(AltitudeTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
AltitudeTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (AltitudeTargettmp < 0)
{
AltitudeTargettmp = 0;
}
GUILayout.Label("m", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.AltitudeTarget == AltitudeTargettmp ? btWhite : btGreen))
{
autopilot.AltitudeTarget = AltitudeTargettmp;
}
GUILayout.EndHorizontal();
if (!autopilot.AltitudeHoldEnabled)
{
bool _VertSpeedHoldEnabled = autopilot.VertSpeedHoldEnabled;
GUILayout.BeginHorizontal();
autopilot.VertSpeedHoldEnabled = GUILayout.Toggle(autopilot.VertSpeedHoldEnabled, "Vertical Speed Hold", GUILayout.Width(140));
if (_VertSpeedHoldEnabled != autopilot.VertSpeedHoldEnabled)
{
if (autopilot.VertSpeedHoldEnabled)
{
autopilot.EnableVertSpeedHold();
}
else
{
autopilot.DisableVertSpeedHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
VertSpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp.text = GUILayout.TextField(VertSpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
VertSpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.VertSpeedTarget == VertSpeedTargettmp ? btWhite : btGreen))
{
autopilot.VertSpeedTarget = VertSpeedTargettmp;
}
GUILayout.EndHorizontal();
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(" Vertical Speed Limit", GUILayout.Width(140));
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
VertSpeedMaxtmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedMaxtmp.text = GUILayout.TextField(VertSpeedMaxtmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
VertSpeedMaxtmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (VertSpeedMaxtmp < 0)
{
VertSpeedMaxtmp = 0;
}
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.VertSpeedMax == VertSpeedMaxtmp ? btWhite : btGreen))
{
autopilot.VertSpeedMax = VertSpeedMaxtmp;
}
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
bool _HeadingHoldEnabled = autopilot.HeadingHoldEnabled;
autopilot.HeadingHoldEnabled = GUILayout.Toggle(autopilot.HeadingHoldEnabled, "Heading Hold", GUILayout.Width(140));
if (_HeadingHoldEnabled != autopilot.HeadingHoldEnabled)
{
if (autopilot.HeadingHoldEnabled)
{
autopilot.EnableHeadingHold();
}
else
{
autopilot.DisableHeadingHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
HeadingTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
HeadingTargettmp.text = GUILayout.TextField(HeadingTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
HeadingTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
HeadingTargettmp = MuUtils.ClampDegrees360(HeadingTargettmp);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.HeadingTarget == HeadingTargettmp ? btWhite : btGreen))
{
autopilot.HeadingTarget = HeadingTargettmp;
}
GUILayout.EndHorizontal();
if (!autopilot.HeadingHoldEnabled)
{
GUILayout.BeginHorizontal();
autopilot.RollHoldEnabled = GUILayout.Toggle(autopilot.RollHoldEnabled, "Roll Hold", GUILayout.Width(140));
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
RollTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollTargettmp.text = GUILayout.TextField(RollTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
RollTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollTargettmp = MuUtils.Clamp(RollTargettmp, -90, 90);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.RollTarget == RollTargettmp ? btWhite : btGreen))
{
autopilot.RollTarget = RollTargettmp;
}
GUILayout.EndHorizontal();
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(" Roll Limit ±", GUILayout.Width(140));
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
RollMaxtmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollMaxtmp.text = GUILayout.TextField(RollMaxtmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
RollMaxtmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollMaxtmp = MuUtils.Clamp(RollMaxtmp, -60, 60);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.RollMax == RollMaxtmp ? btWhite : btGreen))
{
autopilot.RollMax = RollMaxtmp;
}
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
bool _AutoThrustCtrl = autopilot.SpeedHoldEnabled;
autopilot.SpeedHoldEnabled = GUILayout.Toggle(autopilot.SpeedHoldEnabled, "Speed Hold", GUILayout.Width(140));
if (autopilot.SpeedHoldEnabled != _AutoThrustCtrl)
{
if (autopilot.SpeedHoldEnabled)
{
autopilot.EnableSpeedHold();
}
else
{
autopilot.DisableSpeedHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
SpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
SpeedTargettmp.text = GUILayout.TextField(SpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
SpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (SpeedTargettmp < 0)
{
SpeedTargettmp = 0;
}
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.SpeedTarget == SpeedTargettmp ? btWhite : btGreen))
{
autopilot.SpeedTarget = SpeedTargettmp;
}
GUILayout.EndHorizontal();
if (!showpid)
{
if (GUILayout.Button("PID", GUILayout.Width(40)))
{
showpid = true;
}
}
else
{
if (GUILayout.Button("Hide PID", GUILayout.Width(140)))
{
showpid = false;
}
GUILayout.BeginHorizontal();
GUILayout.Label("Accceleration", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.AccKp.text = GUILayout.TextField(autopilot.AccKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.AccKi.text = GUILayout.TextField(autopilot.AccKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.AccKd.text = GUILayout.TextField(autopilot.AccKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.SpeedHoldEnabled)
{
GUILayout.Label("error:" + autopilot.a_err.ToString("F2") + " Target:" + autopilot.RealAccelerationTarget.ToString("F2") + " Cur:" + autopilot.cur_acc.ToString("F2"), GUILayout.ExpandWidth(false));
}
GUILayout.BeginHorizontal();
GUILayout.Label("VertSpeed", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.VerKp.text = GUILayout.TextField(autopilot.VerKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.VerKi.text = GUILayout.TextField(autopilot.VerKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.VerKd.text = GUILayout.TextField(autopilot.VerKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.VertSpeedHoldEnabled)
{
GUILayout.Label("error:" + autopilot.v_err.ToString("F2") + " Target:" + autopilot.RealVertSpeedTarget.ToString("F2") + " Cur:" + vesselState.speedVertical.ToString("F2"), GUILayout.ExpandWidth(false));
}
GUILayout.BeginHorizontal();
GUILayout.Label("Roll", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.RolKp.text = GUILayout.TextField(autopilot.RolKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.RolKi.text = GUILayout.TextField(autopilot.RolKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.RolKd.text = GUILayout.TextField(autopilot.RolKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Yaw", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.YawKp.text = GUILayout.TextField(autopilot.YawKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.YawKi.text = GUILayout.TextField(autopilot.YawKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.YawKd.text = GUILayout.TextField(autopilot.YawKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Yaw Control Limit", GUILayout.ExpandWidth(false));
autopilot.YawLimit.text = GUILayout.TextField(autopilot.YawLimit.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
}
base.WindowGUI(windowID);
}
