andrewdolce

if ( distSq < closest ) {
  // Remember this adjustment as the best so far.
  closest = distSq;
  closestIdx = index;
  closestCoeff = t;
}

andrewdolce

// Compute what the new total force would be with our new
// coefficient.
potential[0] = origin[0] + dirAlongLine[0] * t;
potential[1] = origin[1] + dirAlongLine[1] * t;
potential[2] = origin[2] + dirAlongLine[2] * t;

// Compute the error between the new total force and the
// target.
error[0] = potential[0] - target[0];
error[1] = potential[1] - target[1];

andrewdolce

// Current coeff is already at closest point, so pointless to
// consider further.
if ( Math.abs(coeff - t) < 1e-16 ) { return; }

andrewdolce

// The difference between the target and the origin is the
// "ideal" change that we must try to achieve by adjusting our
// coefficient. We find the coefficient that gets us as close
// as possible by projecting the ideal onto dirAlongLine
// (which represents the actual change we can make.)
var t = ( dirAlongLine[0] * (target[0] - origin[0]) +
          dirAlongLine[1] * (target[1] - origin[1]) +
          dirAlongLine[2] * (target[2] - origin[2]) ) / dirDot;
t = clamp01( t );

andrewdolce

var coeff = thruster.coeff;
// We subtract off our current contribution to get the
// "origin", that is, the forces provided by the other
// thrusters when our coefficent is 0.
origin[0] = current[0] - dirAlongLine[0] * coeff;
origin[1] = current[1] - dirAlongLine[1] * coeff;
origin[2] = current[2] - dirAlongLine[2] * coeff;

andrewdolce

// The thruster's maximum contribution represents the
// direction of a 3D line in space.  Adjusting the coefficient
// will slide the "current" total forces along that line as we
// increase or decrease this thruster's contributions.
dirAlongLine[0] = thruster.localForce[0] * weightF;
dirAlongLine[1] = thruster.localForce[1] * weightR;
dirAlongLine[2] = thruster.torque        * weightT;

var dirDot = dot( dirAlongLine, dirAlongLine );
if ( dirDot === 0 ) { return; }

andrewdolce

// Loop over all the thrusters and figure out which one we
// should adjust to move us closest to the target.
var closest = Infinity;
var closestIdx = 0;
var closestCoeff = 0;
thrusters.forEach(function( thruster, index ) {
  // The thruster's maximum contribution represents the
  // direction of a 3D line in space.  Adjusting the coefficient
  // will slide the "current" total forces along that line as we
  // increase or decrease this thruster's contributions.

andrewdolce

// compute forces based on current thruster coefficients
current[0] = 0;
current[1] = 0;
current[2] = 0;
thrusters.forEach(function( thruster ) {
  var coeff = thruster.coeff;
  current[0] += thruster.localForce[0] * coeff * weightF;
  current[1] += thruster.localForce[1] * coeff * weightR;
  current[2] += thruster.torque        * coeff * weightT;
});

andrewdolce

target[0] = tgtF * weightF;
target[1] = tgtR * weightR;
target[2] = tgtT * weightT;

andrewdolce

// compute the desired longitudinal (F) and lateral (R) forces
var thrustPower = input.thrustPower / 255;
var targetF = 0, targetR = 0;
if ( thrustPower ) {
  var thrustAngleRelative = input.thrustHeading - xf.angle;
  var c = Math.cos( thrustAngleRelative );
  var s = Math.sin( thrustAngleRelative );
  targetF = c * ( c > 0 ? this.maxForward : this.maxBackward );
  targetR = s * ( s > 0 ? this.maxRight   : this.maxLeft     );
}