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 ThrusterSolution(KeyThrustRequest request, ThrustContributionModel model, MassMatrix inertia, double mass)
{
this.Request = request;
this.Model = model;
this.Inertia = inertia;
this.Mass = mass;
}
private void EnsureThrustKeysBuilt_Finish(KeyThrustRequest[] requests)
{
// Remember the mappings between key and desired thrust (this is used to drive the impulse engine)
_keyThrustRequests = requests;
if (this.Thrusters == null || this.Thrusters.Length == 0)
{
_isThrustMapDirty = false;
return;
}
if (_cancelCurrentBalancer != null)
{
_cancelCurrentBalancer.Cancel();
_cancelCurrentBalancer = null;
}
// Remember the current solutions, so they can help get a good start on the new solver
var previous = _thrustLines.Values.ToArray();
_thrustLines.Clear();
_cancelCurrentBalancer = new CancellationTokenSource();
ThrustContributionModel model = new ThrustContributionModel(this.Thrusters, this.PhysicsBody.CenterOfMass);
MassMatrix inertia = this.PhysicsBody.MassMatrix;
double mass = inertia.Mass;
// Several key combos may request the same direction, so group them
var grouped = requests.
ToLookup(KeyThrustRequestComparer);
foreach (var set in grouped)
{
// Create wrappers for this set
ThrusterSolution[] solutionWrappers = set.
Select(o => new ThrusterSolution(o, model, inertia, mass)).
ToArray();
// Store the wrappers
foreach (var wrapper in solutionWrappers)
{
_thrustLines.Add(Tuple.Create(wrapper.Request.Key, wrapper.Request.Shift), wrapper);
}
// This delegate gets called when a better solution is found. Distribute the map to the solution wrappers
var newBestFound = new Action <ThrusterMap>(o =>
{
ThrusterSolutionMap solutionMap = GetThrusterSolutionMap(o, model, inertia, mass);
foreach (ThrusterSolution wrapper in solutionWrappers)
{
wrapper.Map = solutionMap;
}
});
var options = new DiscoverSolutionOptions2 <Tuple <int, int, double> >()
{
//MaxIterations = 2000, //TODO: Find a reasonable stop condition
ThreadShare = _thrustWorkerThread,
};
// Find the previous solution for this request
var prevMatch = previous.FirstOrDefault(o => KeyThrustRequestComparer(set.Key, o.Request));
if (prevMatch != null && prevMatch.Map != null)
{
options.Predefined = new[] { prevMatch.Map.Map.Flattened };
}
// Find the combination of thrusters that push in the requested direction
//ThrustControlUtil.DiscoverSolutionAsync(this, solutionWrappers[0].Request.Linear, solutionWrappers[0].Request.Rotate, _cancelCurrentBalancer.Token, model, newBestFound, null, options);
ThrustControlUtil.DiscoverSolutionAsync2(this, solutionWrappers[0].Request.Linear, solutionWrappers[0].Request.Rotate, _cancelCurrentBalancer.Token, model, newBestFound, options: options);
}
_isThrustMapDirty = false;
}
