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; }