One cool thing about being an indie dev is that I am at complete liberty to share my source code when I see fit. So I'll do just that! These are the relevant parts of the thruster calculation algorithm. Basically I call one of the below functions depending on whether I want to control overall ship's force, acceleration, or velocity. I don't have time to explain how this works in detail beyond the comments I already have, but I may or may not be able to answer specific questions.
/* Acronyms:
* SRF = Ship-Relative Force
* WRF = World-Relative Force
* SRA = Ship-Relative Acceleration
* WRA = World-Relative Acceleration
* SRV = Ship-Relative Velocity
* WRV = World-Relative Velocity
* FSRF = Factored Ship-Relative Force (SRF component-multiplied by SRFFactors)
* NFSRF = Normalized Factored Ship-Relative Force
*/
/// <summary>
/// Calculates all thruster activation levels according to the desired ship-relative force.
/// Call CommitActivationLevels to apply the calculated activations to the thrusters.
/// </summary>
/// <param name="useUnrampedActivationRanges">
/// If true, the calculation will use activation ranges that have not been clamped to their possible deltas this tick.
/// </param>
/// <returns>The actual SRF to which the thrusters have been calculated.</returns>
public Vector3 CalculateDesiredSRF(Vector3 desiredSRF, bool useUnrampedActivationRanges = false)
{
SinCos(Ship.FlightDirection, out float sinRot, out float cosRot);
Vector3 srfFactors = Ship.Rules.ThrusterSRFFactors;
Vector3 _RotateAndFactorSRF(Vector3 srf, bool zeroTorque = false)
{
srf.XY = new Vector2(
(srf.X * cosRot - srf.Y * sinRot) * srfFactors.X,
(srf.X * sinRot + srf.Y * cosRot) * srfFactors.Y);
srf.Z = zeroTorque ? 0 : srf.Z * srfFactors.Z;
return srf;
}
// Zero activation level on all thrusters.
int count = _orderedThrusters.Count;
for(int i = 0; i < count; i++)
_orderedThrusters[i].UncomittedActivationLevel = 0;
// If desired SRF is zero, just stop here.
if(desiredSRF == Vector3.Zero)
return Vector3.Zero;
Vector3 initialDesiredSRF = desiredSRF;
// Get thrusters and compute various force vectors.
TempCalcData calcData = ObjectPool<TempCalcData>.Alloc();
calcData.EnsureCapacity(count);
Vector3[] thrusterSRFs = calcData.ThrusterSRFs;
Vector3[] thrusterFSRFs = calcData.ThrusterFSRFs;
Vector3[] thrusterNFSRFs = calcData.ThrusterNFSRFs;
Range<float>[] thrusterActivationRanges = calcData.ThrusterActivationRanges;
for(int i = 0; i < count; i++)
{
Thruster t = _orderedThrusters[i];
thrusterSRFs[i] = t.GetSRF();
thrusterFSRFs[i] = _RotateAndFactorSRF(thrusterSRFs[i]);
thrusterNFSRFs[i] = thrusterFSRFs[i].Normalize();
thrusterActivationRanges[i] = new Range<float>(
t.Rules.MinActivation.GetValue(t.Part),
t.Rules.MaxActivation.GetValue(t.Part));
if(!useUnrampedActivationRanges)
thrusterActivationRanges[i].Max = Min(thrusterActivationRanges[i].Max, t.ActivationLevel + Sim.FixedUpdater.Interval / t.Rules.ActivationIncreaseTime.GetValue(t.Part));
}
// Repeat for some number of iterations.
for(int iter = 0; iter < Ship.Rules.ThrusterSolverIterations; iter++)
{
// If first iteration, don't factor in torque.
// We do this because of a weakness in the basic algorithm
// that will otherwise prevent balanced thruster combinations from
// firing at full thrust. This way all thrusters will start at full
// for xy movement and then readjust on later iterations
// to take torque into account.
bool zeroTorque = iter == 0;
Vector3 desiredFSRF = _RotateAndFactorSRF(desiredSRF, zeroTorque);
Vector3 desiredNFSRF = desiredFSRF.Normalize();
// Build list of thrusters sorted by their nfsrf's proximity to the desired nfsrf.
using TempList<ThrusterInfo> thrusterInfos = TempList<ThrusterInfo>.Alloc();
for(int i = 0; i < count; i++)
{
Thruster t = _orderedThrusters[i];
Vector3 srf = thrusterSRFs[i];
Vector3 fsrf = zeroTorque ? _RotateAndFactorSRF(thrusterSRFs[i], zeroTorque:true) : thrusterFSRFs[i];
Vector3 nfsrf = thrusterNFSRFs[i];
Range<float> activationRange = thrusterActivationRanges[i];
// Sort based on how well a *potential* change in activation aligns with the current
// desired NFSRF. (If a decrease in activation is possible, use the reverse of the
// thruster's NFSRF.)
float distToDesired = float.MaxValue;
if(t.UncomittedActivationLevel < activationRange.Max)
distToDesired = (nfsrf - desiredNFSRF).LengthSquared;
if(t.UncomittedActivationLevel > activationRange.Min)
distToDesired = Min(distToDesired, (-nfsrf - desiredNFSRF).LengthSquared);
thrusterInfos.Add(new ThrusterInfo(t, srf, fsrf, distToDesired));
}
thrusterInfos.Sort();
// Set the activation level of each thruster in the list,
// subtracting the new thrust from the desired thrust.
foreach(ThrusterInfo st in thrusterInfos)
{
// Project desired srf onto thruster srf and divide by the thruster's magnitude
// to get its optimal change in activation level.
float oldActivation = st.Thruster.UncomittedActivationLevel;
float activationDelta = desiredFSRF.Dot(st.FSRF) / st.FSRF.LengthSquared;
st.Thruster.UncomittedActivationLevel += activationDelta;
float activationDiff = st.Thruster.UncomittedActivationLevel - oldActivation;
desiredSRF -= activationDiff * st.SRF;
desiredFSRF -= activationDiff * st.FSRF;
}
}
ObjectPool<TempCalcData>.Recycle(calcData);
Vector3 actualSRF = initialDesiredSRF - desiredSRF;
// If we fail to calculate activations that get us closer to the desired, just do nothing.
if(_RotateAndFactorSRF(actualSRF).Dot(_RotateAndFactorSRF(initialDesiredSRF)) < 0)
return CalculateDesiredSRF(Vector3.Zero, useUnrampedActivationRanges);
return actualSRF;
}
/// <summary>
/// Calculates all thruster activation levels according to the desired world-relative force.
/// Call CommitActivationLevels to apply the calculated activations to the thrusters.
/// </summary>
/// <param name="useUnrampedActivationRanges">
/// If true, the calculation will use activation ranges that have not been clamped to their possible deltas this tick.
/// </param>
/// <returns>The actual WRF to which the thrusters have been calculated.</returns>
public Vector3 CalculateDesiredWRF(Vector3 desiredWRF, bool useUnrampedActivationRanges = false)
{
// Rotate desired WRF by the ship's angle.
float rot = -Ship.DetRotation;
SinCos(rot, out float cosRot, out float sinRot);
Vector3 desiredSRF = new Vector3(
desiredWRF.X * cosRot - desiredWRF.Y * sinRot,
desiredWRF.X * sinRot + desiredWRF.Y * cosRot,
desiredWRF.Z);
Vector3 actualSRF = CalculateDesiredSRF(desiredSRF, useUnrampedActivationRanges);
// Rotate actual SRF by opposite of ship's angle to get actual WRF.
SinCos(-rot, out sinRot, out cosRot);
return new Vector3(
actualSRF.X * cosRot - actualSRF.Y * sinRot,
actualSRF.X * sinRot + actualSRF.Y * cosRot,
actualSRF.Z);
}
/// <summary>
/// Calculates all thruster activation levels according to the desired ship-relative acceleration.
/// Call CommitActivationLevels to apply the calculated activations to the thrusters.
/// </summary>
/// <param name="useUnrampedActivationRanges">
/// If true, the calculation will use activation ranges that have not been clamped to their possible deltas this tick.
/// </param>
/// <returns>The actual SRA to which the thrusters have been calculated.</returns>
public Vector3 CalculateDesiredSRA(Vector3 desiredSRA, bool useUnrampedActivationRanges = false)
{
float mass = Ship.Physics.Body.Mass;
float inertia = Ship.Physics.Body.IntertiaAboutCentroid;
Vector3 desiredSRF = new Vector3(
desiredSRA.X * mass,
desiredSRA.Y * mass,
desiredSRA.Z * inertia);
Vector3 actualSRF = CalculateDesiredSRF(desiredSRF, useUnrampedActivationRanges);
return new Vector3(
actualSRF.X / mass,
actualSRF.Y / mass,
actualSRF.Z / inertia);
}
/// <summary>
/// Calculates all thruster activation levels according to the desired ship-relative acceleration.
/// Call CommitActivationLevels to apply the calculated activations to the thrusters.
/// </summary>
/// <param name="useUnrampedActivationRanges">
/// If true, the calculation will use activation ranges that have not been clamped to their possible deltas this tick.
/// </param>
/// <returns>The actual WRA to which the thrusters have been calculated.</returns>
public Vector3 CalculateDesiredWRA(Vector3 desiredWRA, bool useUnrampedActivationRanges = false)
{
float mass = Ship.Physics.Body.Mass;
float inertia = Ship.Physics.Body.IntertiaAboutCentroid;
Vector3 desiredWRF = new Vector3(
desiredWRA.X * mass,
desiredWRA.Y * mass,
desiredWRA.Z * inertia);
Vector3 actualWRF = CalculateDesiredWRF(desiredWRF, useUnrampedActivationRanges);
return new Vector3(
actualWRF.X / mass,
actualWRF.Y / mass,
actualWRF.Z / inertia);
}
/// <summary>
/// Calculates all thruster activation levels according to the desired ship-relative velocity.
/// Call CommitActivationLevels to apply the calculated activations to the thrusters.
/// </summary>
/// <param name="useUnrampedActivationRanges">
/// If true, the calculation will use activation ranges that have not been clamped to their possible deltas this tick.
/// </param>
public void CalculateDesiredSRV(Vector3 desiredSRV, bool useUnrampedActivationRanges = false)
{
Vector3 desiredSRA = desiredSRV - GetCurrentSRV();
CalculateDesiredSRA(desiredSRA, useUnrampedActivationRanges);
}
/// <summary>
/// Returns the current ship-relative velocity.
/// </summary>
public Vector3 GetCurrentSRV()
{
Vector3 curWRV = new Vector3(
Ship.Physics.Body.LinearVelocity,
Ship.Physics.Body.AngularVelocity);
// Rotate curWRV by ship's angle.
float rot = -Ship.DetRotation;
SinCos(rot, out float sinRot, out float cosRot);
return new Vector3(
curWRV.X * cosRot - curWRV.Y * sinRot,
curWRV.X * sinRot + curWRV.Y * cosRot,
curWRV.Z);
}
/// <summary>
/// Calculates all thruster activation levels according to the desired world-relative velocity.
/// Call CommitActivationLevels to apply the calculated activations to the thrusters.
/// </summary>
/// <param name="useUnrampedActivationRanges">
/// If true, the calculation will use activation ranges that have not been clamped to their possible deltas this tick.
/// </param>
public void CalculateDesiredWRV(Vector3 desiredWRV, bool useUnrampedActivationRanges = false)
{
Vector3 curWRV = new Vector3(
Ship.Physics.Body.LinearVelocity,
Ship.Physics.Body.AngularVelocity);
Vector3 desiredWRA = (desiredWRV - curWRV);
CalculateDesiredWRA(desiredWRA, useUnrampedActivationRanges);
}