// The list of throttles is ordered under the assumption that you iterate
// over the vessel as follows:
// foreach part in vessel.parts:
// foreach rcsModule in part.Modules.OfType<ModuleRCS>:
// ...
// Note that rotation balancing is not supported at the moment.
public void GetThrottles(Vessel vessel, VesselState state, Vector3 direction,
out double[] throttles, out List <RCSSolver.Thruster> thrustersOut)
{
thrustersOut = callerThrusters;
Vector3 rotation = Vector3.zero;
var rcsBalancer = VesselExtensions.GetMasterMechJeb(vessel).rcsbal;
// Update vessel info if needed.
CheckVessel(vessel, state);
Vector3 dir = direction.normalized;
RCSSolverKey key = new RCSSolverKey(ref dir, rotation);
if (thrusters.Count == 0)
{
throttles = double0;
}
else if (direction == Vector3.zero)
{
throttles = originalThrottles;
}
else if (results.TryGetValue(key, out throttles))
{
cacheHits++;
}
else
{
// This task hasn't been calculated. We'll handle that here.
// Meanwhile, TryGetValue() will have set 'throttles' to null, but
// we'll make it a 0-element array instead to avoid null checks.
cacheMisses++;
throttles = double0;
if (pending.Contains(key))
{
// We've submitted this key before, so we need to check the
// results queue.
while (resultsQueue.Count > 0)
{
SolverResult sr = (SolverResult)resultsQueue.Dequeue();
results[sr.key] = sr.throttles;
pending.Remove(sr.key);
if (sr.key == key)
{
throttles = sr.throttles;
}
}
}
else
{
// This task was neither calculated nor pending, so we've never
// submitted it. Do so!
pending.Add(key);
tasks.Enqueue(new SolverTask(key, dir, rotation));
workEvent.Set();
}
}
// Return a copy of the array to make sure ours isn't modified.
throttles = (double[])throttles.Clone();
}
internal static bool InitTypes()
{
AssemblyLoader.loadedAssemblies.TypeOperation(t => {
switch (t.FullName)
{
case "MuMech.MechJebCore":
type = t;
getComputerModule = t.GetMethod("GetComputerModule", new Type[] { typeof(string) });
break;
default:
bool unused = AscentAutopilot.InitTypes(t) || ComputerModule.InitTypes(t) || EditableVariables.InitTypes(t) || Operation.InitTypes(t) || TimeSelector.InitTypes(t) || VesselExtensions.InitTypes(t);
break;
}
});
return(type != null);
}
private void CheckVessel(Vessel vessel, VesselState state)
{
bool changed = false;
var rcsBalancer = VesselExtensions.GetMasterMechJeb(vessel).rcsbal;
if (vessel.parts.Count != lastPartCount)
{
lastPartCount = vessel.parts.Count;
changed = true;
}
// Make sure all thrusters are still enabled, because if they're not,
// our calculations will be wrong.
for (int i = 0; i < thrusters.Count; i++)
{
if (!thrusters[i].partModule.isEnabled)
{
changed = true;
break;
}
}
// Likewise, make sure any previously-disabled RCS modules are still
// disabled.
foreach (var pm in lastDisabled)
{
if (pm.isEnabled)
{
changed = true;
break;
}
}
// See if the CoM has moved too much.
Rigidbody rootPartBody = vessel.rootPart.rigidbody;
if (rootPartBody != null)
{
// But how much is "too much"? Well, it probably has something to do
// with the ship's moment of inertia (MoI). Let's say the distance
// 'd' that the CoM is allowed to shift without a reset is:
//
// d = moi * x + c
//
// where 'moi' is the magnitude of the ship's moment of inertia and
// 'x' and 'c' are tuning parameters to be determined.
//
// Using a few actual KSP ships, I burned RCS fuel (or moved fuel
// from one tank to another) to see how far the CoM could shift
// before the the rotation error on translation became annoying.
// I came up with roughly:
//
// d moi
// 0.005 2.34
// 0.04 11.90
// 0.07 19.96
//
// I then halved each 'd' value, because we'd like to address this
// problem -before- it becomes annoying. Least-squares linear
// regression on the (moi, d/2) pairs gives the following (with
// adjusted R^2 = 0.999966):
//
// moi = 542.268 d + 1.00654
// d = (moi - 1) / 542
//
// So the numbers below have some basis in reality. =)
// Assume MoI magnitude is always >=2.34, since that's all I tested.
comErrorThreshold = (Math.Max(state.MoI.magnitude, 2.34) - 1) / 542;
Vector3 comState = state.CoM;
Vector3 rootPos = state.rootPartPos;
Vector3 com = WorldToVessel(vessel, comState - rootPos);
double thisComErr = (lastCoM - com).magnitude;
maxComError = Math.Max(maxComError, thisComErr);
_comError.value = thisComErr;
if (_comError > comErrorThreshold)
{
lastCoM = com;
changed = true;
}
}
if (!changed)
{
return;
}
// Something about the vessel has changed. We need to reset everything.
lastDisabled.Clear();
// ModuleRCS has no originalThrusterPower attribute, so we have
// to explicitly reset it.
ResetThrusterForces();
// Rebuild the list of thrusters.
var ts = new List <RCSSolver.Thruster>();
foreach (Part p in vessel.parts)
{
foreach (ModuleRCS pm in p.Modules.OfType <ModuleRCS>())
{
if (!pm.isEnabled)
{
// Keep track of this module so we'll know if it's enabled.
lastDisabled.Add(pm);
}
else if (p.Rigidbody != null && !pm.isJustForShow)
{
Vector3 pos = VesselRelativePos(state.CoM, vessel, p);
// Create a single RCSSolver.Thruster for this part. This
// requires some assumptions about how the game's RCS code will
// drive the individual thrusters (which we can't control).
Vector3[] thrustDirs = new Vector3[pm.thrusterTransforms.Count];
Quaternion rotationQuat = Quaternion.Inverse(vessel.GetTransform().rotation);
for (int i = 0; i < pm.thrusterTransforms.Count; i++)
{
thrustDirs[i] = (rotationQuat * -pm.thrusterTransforms[i].up).normalized;
}
ts.Add(new RCSSolver.Thruster(pos, thrustDirs, p, pm));
}
}
}
callerThrusters.Clear();
originalThrottles = new double[ts.Count];
zeroThrottles = new double[ts.Count];
for (int i = 0; i < ts.Count; i++)
{
originalThrottles[i] = ts[i].originalForce;
zeroThrottles[i] = 0;
callerThrusters.Add(ts[i]);
}
thrusters = ts;
ClearResults();
}
public override float ErrorFromSetPoint(float setPoint)
{
return(VesselExtensions.AngleSubtract(ProcessVariable, setPoint));
}
