static void MaxThrust(double[] x, ref double func, double[] grad, object obj)
{
List <VesselState.EngineWrapper> el = (List <VesselState.EngineWrapper>)obj;
func = 0;
for (int i = 0, j = 0; j < el.Count; j++)
{
VesselState.EngineWrapper e = el[j];
if (!e.engine.throttleLocked)
{
func -= el[j].maxVariableForce.y * x[i];
grad[i] = -el[j].maxVariableForce.y;
i++;
}
}
}
public bool ComputeDifferentialThrottle(Vector3d torque)
{
List <VesselState.EngineWrapper> engines = vesselState.enginesWrappers;
int n = engines.Count(eng => !eng.engine.throttleLocked);
if (n < 3)
{
for (int i = 0; i < engines.Count; i++)
{
VesselState.EngineWrapper e = engines[i];
e.thrustRatio = 1;
}
return(false);
}
double[,] C = new double[2 + 2 * n, n + 1];
int[] CT = new int[2 + 2 * n];
float mainThrottle = vessel.ctrlState.mainThrottle;
// FIXME: the solver will throw an exception if the commanded torque is not realisable,
// clamp the commanded torque to half the possible torque for now
if (double.IsNaN(vesselState.torqueDiffThrottle.x))
{
vesselState.torqueDiffThrottle.x = 0;
}
if (double.IsNaN(vesselState.torqueDiffThrottle.z))
{
vesselState.torqueDiffThrottle.z = 0;
}
C[0, n] = Mathf.Clamp((float)torque.x, -(float)vesselState.torqueDiffThrottle.x * mainThrottle / 2, (float)vesselState.torqueDiffThrottle.x * mainThrottle / 2);
C[1, n] = Mathf.Clamp((float)torque.z, -(float)vesselState.torqueDiffThrottle.z * mainThrottle / 2, (float)vesselState.torqueDiffThrottle.z * mainThrottle / 2);
for (int i = 0, j = 0; j < engines.Count; j++)
{
var e = engines[j];
C[0, n] -= e.constantTorque.x;
C[1, n] -= e.constantTorque.z;
if (!e.engine.throttleLocked)
{
C[0, i] = e.maxVariableTorque.x;
//C[1,j] = e.maxVariableTorque.y;
C[1, i] = e.maxVariableTorque.z;
C[2 + 2 * i, i] = 1;
C[2 + 2 * i, n] = 1;
CT[2 + 2 * i] = -1;
C[3 + 2 * i, i] = 1;
C[3 + 2 * i, n] = 0;
CT[3 + 2 * i] = 1;
i++;
}
}
double[] w = new double[0];
double[,] u = new double[0, 0];
double[,] vt = new double[0, 0];
alglib.svd.rmatrixsvd(C, 2, n, 0, 0, 2, ref w, ref u, ref vt);
if (w[0] >= 10 * w[1])
{
for (int i = 0; i < engines.Count; i++)
{
VesselState.EngineWrapper e = engines[i];
e.thrustRatio = 1;
}
return(false);
}
// Multiply by mainThrottle later to compute the singular value decomposition correctly
for (int i = 0; i < n; i++)
{
C[0, i] *= mainThrottle;
C[1, i] *= mainThrottle;
}
double[] x = new double[n];
alglib.minbleicstate state;
alglib.minbleicreport rep;
try
{
alglib.minbleiccreate(x, out state);
alglib.minbleicsetlc(state, C, CT);
alglib.minbleicoptimize(state, MaxThrust, null, engines);
alglib.minbleicresults(state, out x, out rep);
}
catch
{
return(false);
}
if (x.Any(val => double.IsNaN(val)))
{
return(false);
}
for (int i = 0, j = 0; j < engines.Count; j++)
{
if (!engines[j].engine.throttleLocked)
{
engines[j].thrustRatio = (float)x[i];
i++;
}
}
return(true);
}