private static ThrusterSolutionMap GetThrusterSolutionMap(ThrusterMap map, ThrustContributionModel model, MassMatrix inertia, double mass)
{
// Add up the forces
Vector3D sumLinearForce = new Vector3D();
Vector3D sumTorque = new Vector3D();
foreach (ThrusterSetting thruster in map.UsedThrusters)
{
var contribution = model.Contributions.FirstOrDefault(o => o.Item1 == thruster.ThrusterIndex && o.Item2 == thruster.SubIndex);
if (contribution == null)
{
throw new ApplicationException(string.Format("Didn't find contribution for thruster: {0}, {1}", thruster.ThrusterIndex, thruster.SubIndex));
}
sumLinearForce += contribution.Item3.TranslationForce * thruster.Percent;
sumTorque += contribution.Item3.Torque * thruster.Percent;
}
// Divide by mass
//F=MA, A=F/M
double accel = sumLinearForce.Length / mass;
Vector3D projected = inertia.Inertia.GetProjectedVector(sumTorque);
double angAccel = sumTorque.Length / projected.Length;
if (Math1D.IsInvalid(angAccel))
{
angAccel = 0; // this happens when there is no net torque
}
return(new ThrusterSolutionMap(map, accel, angAccel));
}
public override void Update_MainThread(double elapsedTime)
{
ThrusterMap forwardMap = this.ForwardMap;
if (forwardMap != null)
{
//NOTE: This isn't range checking. The map's flattened should have every thruster and thruster's direction
foreach (var thruster in forwardMap.Flattened.ToLookup(o => o.Item1))
{
double[] percents = thruster.
OrderBy(o => o.Item2).
Select(o => o.Item3).
ToArray();
this.Thrusters[thruster.Key].Percents = percents;
}
}
else
{
foreach (Thruster thruster in this.Thrusters)
{
thruster.Percents = Enumerable.Range(0, thruster.ThrusterDirectionsModel.Length).
Select(o => 1d).
ToArray();
}
}
base.Update_MainThread(elapsedTime);
}
