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;
}
public override void Update_MainThread(double elapsedTime)
{
#region clear thrusters/impulse
// Reset the thrusters
//TODO: It's ineficient to do this every tick
if (Thrusters != null)
{
foreach (Thruster thruster in Thrusters)
{
thruster.Percents = new double[thruster.ThrusterDirectionsModel.Length];
}
}
if (ImpulseEngines != null)
{
foreach (ImpulseEngine impulse in ImpulseEngines)
{
impulse.SetDesiredDirection(null); // can't call clear, because then it will listen to its neurons
}
}
#endregion
if (_downKeys.Count > 0)
{
EnsureThrustKeysBuilt_YISUP();
List <(Vector3D?, Vector3D?)> impulseEngineCommand = new List <(Vector3D?, Vector3D?)>();
foreach (var key in _downKeys)
{
#region thrusters
ThrusterSolution solution;
if (_thrustLines.TryGetValue(key, out solution) || _thrustLines.TryGetValue(Tuple.Create(key.Item1, (bool?)null), out solution)) // _downKeys will always have the bool set to true or false, but _thrustLines may have it stored as a null (null means ignore shift key)
{
ThrusterSolutionMap map = solution.Map;
if (map != null)
{
//TODO: The thrust map is normalized for maximum thrust. If they want full thrust immediately, use it. Otherwise roll on the thrust as they
//hold the key in (start at a fixed min accel, then gradient up to full after a second or two)
//solution.Request.Max
double percentLin = 1d;
if (solution.Request.Linear != null && solution.Request.MaxLinear != null && map.LinearAccel > solution.Request.MaxLinear.Value)
{
percentLin = solution.Request.MaxLinear.Value / map.LinearAccel;
}
double percentRot = 1d;
if (solution.Request.Rotate != null && solution.Request.MaxRotate != null && map.RotateAccel > solution.Request.MaxRotate.Value)
{
percentRot = solution.Request.MaxRotate.Value / map.RotateAccel;
}
double percent = Math.Min(percentLin, percentRot);
foreach (ThrusterSetting thruster in map.Map.UsedThrusters)
{
//NOTE: If this percent goes over 1, the Fire method will cap it. Any future control theory logic will get confused, because not all of what it said was actually used
thruster.Thruster.Percents[thruster.SubIndex] += thruster.Percent * percent;
}
}
}
#endregion
#region impulse engines
if (ImpulseEngines != null && ImpulseEngines.Length > 0)
{
// Figure out what vector (linear and/or torque) is associated with this key
var match = _keyThrustRequests.
Where(o => o.Key == key.Item1).
Select(o => new
{
Request = o,
Score = o.Shift == null || key.Item2 == null ? 1 : // shift press is null, this is the middle score
o.Shift.Value == key.Item2.Value ? 0 : // shift presses match, this is the best score
2, // shift presses don't match, this is the worst score
}).
OrderBy(o => o.Score).
Select(o => o.Request).
FirstOrDefault();
if (match != null)
{
// Impulse engine wants the vectors to be percents (length up to 1)
impulseEngineCommand.Add((match.Linear, match.Rotate));
}
}
#endregion
}
#region impulse engines
if (ImpulseEngines != null && ImpulseEngines.Length > 0 && impulseEngineCommand.Count > 0)
{
var impulseCommand = impulseEngineCommand.ToArray();
foreach (ImpulseEngine impulseEngine in this.ImpulseEngines)
{
impulseEngine.SetDesiredDirection(impulseCommand);
}
}
#endregion
}
base.Update_MainThread(elapsedTime);
}
