コード例 #1
0
        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);
        }