static void MaxThrust(double[] x, ref double func, double[] grad, object obj)
{
List <EngineWrapper> el = (List <EngineWrapper>)obj;
func = 0;
for (int i = 0, j = 0; j < el.Count; j++)
{
EngineWrapper e = el[j];
if (!e.engine.throttleLocked)
{
func -= el[j].maxVariableForce.y * x[i];
grad[i] = -el[j].maxVariableForce.y;
i++;
}
}
}
public void DisableDifferentialThrottle()
{
for (int i = 0; i < vessel.parts.Count; i++)
{
Part p = vessel.parts[i];
for (int j = 0; j < p.Modules.Count; j++)
{
PartModule pm = p.Modules[j];
if (pm is ModuleEngines)
{
ModuleEngines e = (ModuleEngines)pm;
EngineWrapper engine = new EngineWrapper(e);
engine.thrustRatio = 1;
}
}
}
}
public bool ComputeDifferentialThrottle(Vector3d torque)
{
List <EngineWrapper> engines = new List <EngineWrapper>();
for (int i = 0; i < vessel.parts.Count; i++)
{
Part p = vessel.parts[i];
for (int j = 0; j < p.Modules.Count; j++)
{
PartModule pm = p.Modules[j];
if (pm is ModuleEngines)
{
ModuleEngines e = (ModuleEngines)pm;
EngineWrapper engine = new EngineWrapper(e);
if (e.EngineIgnited && !e.flameout && e.enabled)
{
engine.UpdateForceAndTorque(vesselState.CoM);
engines.Add(engine);
}
}
}
}
int n = engines.Count(eng => !eng.engine.throttleLocked);
if (n < 3)
{
for (int i = 0; i < engines.Count; i++)
{
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.torqueFromDiffThrottle.x))
{
vesselState.torqueFromDiffThrottle.x = 0;
}
if (double.IsNaN(vesselState.torqueFromDiffThrottle.z))
{
vesselState.torqueFromDiffThrottle.z = 0;
}
C[0, n] = Mathf.Clamp((float)torque.x, -(float)vesselState.torqueFromDiffThrottle.x * mainThrottle / 2, (float)vesselState.torqueFromDiffThrottle.x * mainThrottle / 2);
C[1, n] = Mathf.Clamp((float)torque.z, -(float)vesselState.torqueFromDiffThrottle.z * mainThrottle / 2, (float)vesselState.torqueFromDiffThrottle.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++)
{
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);
}
public void DisableDifferentialThrottle()
{
for (int i = 0; i < vessel.parts.Count; i++)
{
Part p = vessel.parts[i];
for (int j = 0; j < p.Modules.Count; j++)
{
PartModule pm = p.Modules[j];
if (pm is ModuleEngines)
{
ModuleEngines e = (ModuleEngines)pm;
EngineWrapper engine = new EngineWrapper(e);
engine.thrustRatio = 1;
}
}
}
}
public bool ComputeDifferentialThrottle(Vector3d torque)
{
List<EngineWrapper> engines = new List<EngineWrapper>();
for (int i = 0; i < vessel.parts.Count; i++)
{
Part p = vessel.parts[i];
for (int j = 0; j < p.Modules.Count; j++)
{
PartModule pm = p.Modules[j];
if (pm is ModuleEngines)
{
ModuleEngines e = (ModuleEngines)pm;
EngineWrapper engine = new EngineWrapper(e);
if (e.EngineIgnited && !e.flameout && e.enabled)
{
engine.UpdateForceAndTorque(vesselState.CoM);
engines.Add(engine);
}
}
}
}
int n = engines.Count(eng => !eng.engine.throttleLocked);
if (n < 3)
{
for (int i = 0; i < engines.Count; i++)
{
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.torqueFromDiffThrottle.x)) vesselState.torqueFromDiffThrottle.x = 0;
if (double.IsNaN(vesselState.torqueFromDiffThrottle.z)) vesselState.torqueFromDiffThrottle.z = 0;
C[0, n] = Mathf.Clamp((float)torque.x, -(float)vesselState.torqueFromDiffThrottle.x * mainThrottle / 2, (float)vesselState.torqueFromDiffThrottle.x * mainThrottle / 2);
C[1, n] = Mathf.Clamp((float)torque.z, -(float)vesselState.torqueFromDiffThrottle.z * mainThrottle / 2, (float)vesselState.torqueFromDiffThrottle.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++)
{
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;
}
