public double[] run(List <Thruster> fullThrusters, Vector3 direction, Vector3 rotation)
{
direction = direction.normalized;
int fullCount = fullThrusters.Count;
List <Thruster> thrusters = new List <Thruster>();
Vector3[] thrustForces = new Vector3[fullCount];
// Initialize the matrix based on thruster directions.
for (int i = 0; i < fullCount; i++)
{
Thruster thruster = fullThrusters[i];
thrustForces[i] = thruster.GetThrust(direction, rotation);
if (thrustForces[i].magnitude > 0)
{
thrusters.Add(thruster);
}
}
int count = thrusters.Count;
if (count == 0)
{
return(null);
}
// We want to minimize torque (3 values), translation error (3 values),
// and any thrust that's wasted due to not being toward 'direction' (1
// value). We also have a value to make sure there's always -some-
// thrust.
A = new double[PARAMLength, count];
B = new double[PARAMLength];
for (int i = 0; i < B.Length; i++)
{
B[i] = 0;
}
B[B.Length - 1] = 0.001 * count;
double[] x = new double[count];
double[] bndl = new double[count];
double[] bndu = new double[count];
// Initialize the matrix based on thruster directions.
int tIdx = -1;
for (int i = 0; i < fullCount; i++)
{
Vector3 thrust = thrustForces[i];
if (thrust.magnitude == 0)
{
continue;
}
Thruster thruster = thrusters[++tIdx];
Vector3 torque = thruster.GetTorque(thrust);
Vector3 thrustNorm = thrust.normalized;
Vector3 torqueErr = torque - rotation;
Vector3 transErr = thrustNorm - direction;
// Waste is a value from [0..2] indicating how much thrust is being
// wasted due to not being toward 'direction':
// 0: perfectly aligned with direction
// 1: perpendicular to direction
// 2: perfectly opposite direction
float waste = 1 - Vector3.Dot(thrustNorm, direction);
if (waste < wasteThreshold)
{
waste = 0;
}
A[0, tIdx] = torqueErr.x * factorTorque;
A[1, tIdx] = torqueErr.y * factorTorque;
A[2, tIdx] = torqueErr.z * factorTorque;
A[3, tIdx] = transErr.x * factorTranslate;
A[4, tIdx] = transErr.y * factorTranslate;
A[5, tIdx] = transErr.z * factorTranslate;
A[6, tIdx] = waste * factorWaste;
A[7, tIdx] = 0.001;
x[tIdx] = 1;
bndl[tIdx] = 0;
bndu[tIdx] = 1;
}
alglib.minbleicstate state;
alglib.minbleicreport rep;
const double epsg = 0.01;
const double epsf = 0;
const double epsx = 0;
const double diffstep = 1.0e-6;
const int maxits = 0;
double[] throttles;
alglib.minbleiccreatef(x, diffstep, out state);
alglib.minbleicsetbc(state, bndl, bndu);
alglib.minbleicsetcond(state, epsg, epsf, epsx, maxits);
alglib.minbleicoptimize(state, cost_func, null, null);
alglib.minbleicresults(state, out throttles, out rep);
double m = throttles.Max();
if (m > 0)
{
for (int i = 0; i < count; i++)
{
throttles[i] /= m;
}
}
double[] fullThrottles = new double[fullCount];
int j = 0;
for (int i = 0; i < fullCount; i++)
{
fullThrottles[i] = (thrustForces[i].magnitude == 0) ? 0 : throttles[j++];
}
return(fullThrottles);
}