public ThrusterSolution(KeyThrustRequest request, ThrustContributionModel model, MassMatrix inertia, double mass)
{
this.Request = request;
this.Model = model;
this.Inertia = inertia;
this.Mass = mass;
}
private double[] GetRHS()
{
double[] RHSvec;
BlockMsrMatrix MassMatrix;
RHSvec = this.Op_Affine.CloneAs();
RHSvec.ScaleV(-1.0);
MassMatrix = this.Op_mass.GetMassMatrix(this.u.Mapping, new double[] { 1.0 }, false, this.LsTrk.SpeciesIdS.ToArray());
MassMatrix.SpMV(1.0, this.rhs.CoordinateVector, 1.0, RHSvec);
return(RHSvec);
}
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);
}
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;
}
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 static IEnumerable <ThrusterSetting> EnsureThrustKeysBuilt_Rotate(Vector3D axis, ThrustContribution[] contributions, double maxAcceleration, MassMatrix inertia)
{
axis = axis.ToUnit(); // doing this so the dot product can be used as a percent
List <Tuple <ThrustContribution, double> > retVal = new List <Tuple <ThrustContribution, double> >();
// Get a list of thrusters that will contribute to the direction
foreach (ThrustContribution contribution in contributions)
{
double dot = Vector3D.DotProduct(contribution.TorqueUnit, axis);
if (dot > .5d)
{
//retVal.Add(new ThrusterSetting(contribution.Thruster, contribution.Index, 1)); // for now, just do 100%
retVal.Add(Tuple.Create(contribution, contribution.TorqueLength * dot));
}
}
retVal = FilterThrusts(retVal);
#region Reduce Percent
double percent = 1;
if (retVal.Count > 0)
{
double torque = retVal.Sum(o => o.Item2);
// A=F/M
//double accel = torque / (Vector3D.DotProduct(inertia.Inertia, axis) * inertia.Mass);
double accel = torque / Math.Abs(Vector3D.DotProduct(inertia.Inertia, axis));
if (accel > maxAcceleration)
{
percent = maxAcceleration / accel;
}
}
#endregion
return(retVal.Select(o => new ThrusterSetting(o.Item1.Thruster, o.Item1.Index, percent)).ToArray()); // commiting to array so that this linq isn't rerun each time it's iterated over
}
private void EnsureThrustKeysBuilt()
{
if (!_isThrustMapDirty)
{
return;
}
if (this.Thrusters == null || this.Thrusters.Length == 0)
{
_isThrustMapDirty = false;
return;
}
_thrustLines.Clear();
ThrustContribution[] contributions = GetThrusterContributions(this.Thrusters, this.PhysicsBody.CenterOfMass);
MassMatrix inertia = this.PhysicsBody.MassMatrix;
double mass = inertia.Mass;
#region Linear
double?maxAccel = this.MaxAcceleration_Linear;
var directions = new[]
{
//new { Direction = new Vector3D(0, 1, 0), Key = Key.W, Shift = (bool?)null, Max = (double?)null },
new { Direction = new Vector3D(0, 1, 0), Key = Key.W, Shift = (bool?)false, Max = (double?)null },
new { Direction = new Vector3D(0, 1, 0), Key = Key.Up, Shift = (bool?)true, Max = (double?)null },
new { Direction = new Vector3D(0, 1, 0), Key = Key.Up, Shift = (bool?)false, Max = maxAccel },
//new { Direction = new Vector3D(0, -1, 0), Key = Key.S, Shift = (bool?)null, Max = (double?)null },
new { Direction = new Vector3D(0, -1, 0), Key = Key.S, Shift = (bool?)false, Max = (double?)null },
new { Direction = new Vector3D(0, -1, 0), Key = Key.Down, Shift = (bool?)true, Max = (double?)null },
new { Direction = new Vector3D(0, -1, 0), Key = Key.Down, Shift = (bool?)false, Max = maxAccel },
new { Direction = new Vector3D(-1, 0, 0), Key = Key.A, Shift = (bool?)true, Max = (double?)null },
new { Direction = new Vector3D(-1, 0, 0), Key = Key.A, Shift = (bool?)false, Max = maxAccel },
new { Direction = new Vector3D(1, 0, 0), Key = Key.D, Shift = (bool?)true, Max = (double?)null },
new { Direction = new Vector3D(1, 0, 0), Key = Key.D, Shift = (bool?)false, Max = maxAccel },
};
foreach (var direction in directions)
{
IEnumerable <ThrusterSetting> thrusters;
if (direction.Max == null)
{
thrusters = EnsureThrustKeysBuilt_Linear(direction.Direction, contributions);
}
else
{
thrusters = EnsureThrustKeysBuilt_Linear(direction.Direction, contributions, direction.Max.Value, mass);
}
_thrustLines.Add(Tuple.Create(direction.Key, direction.Shift), thrusters);
}
#endregion
#region Rotation
maxAccel = this.MaxAcceleration_Rotate;
var torques = new[]
{
new { Torque = new Vector3D(0, 0, 1), Key = Key.Left, Shift = (bool?)true, Max = (double?)null },
new { Torque = new Vector3D(0, 0, 1), Key = Key.Left, Shift = (bool?)false, Max = maxAccel },
new { Torque = new Vector3D(0, 0, -1), Key = Key.Right, Shift = (bool?)true, Max = (double?)null },
new { Torque = new Vector3D(0, 0, -1), Key = Key.Right, Shift = (bool?)false, Max = maxAccel },
new { Torque = new Vector3D(0, -1, 0), Key = Key.Q, Shift = (bool?)false, Max = (double?)null }, // roll left
new { Torque = new Vector3D(0, 1, 0), Key = Key.E, Shift = (bool?)false, Max = (double?)null },
//new { Torque = new Vector3D(-1, 0, 0), Key = Key.Q, Shift = (bool?)true, Max = (double?)null }, // pitch down
//new { Torque = new Vector3D(1, 0, 0), Key = Key.E, Shift = (bool?)true, Max = (double?)null },
new { Torque = new Vector3D(-1, 0, 0), Key = Key.W, Shift = (bool?)true, Max = (double?)null }, // pitch down
new { Torque = new Vector3D(1, 0, 0), Key = Key.S, Shift = (bool?)true, Max = (double?)null },
};
foreach (var torque in torques)
{
IEnumerable <ThrusterSetting> thrusters;
if (torque.Max == null)
{
thrusters = EnsureThrustKeysBuilt_Rotate(torque.Torque, contributions);
}
else
{
thrusters = EnsureThrustKeysBuilt_Rotate(torque.Torque, contributions, torque.Max.Value, inertia);
}
_thrustLines.Add(Tuple.Create(torque.Key, torque.Shift), thrusters);
}
#endregion
_isThrustMapDirty = false;
}
internal void EnsureThrustSetsCalculated()
{
if (_contributions != null)
{
return;
}
_shipCenterMass = _bot.PhysicsBody.CenterOfMass;
_shipMassMatrix = _bot.PhysicsBody.MassMatrix;
_contributions = GetThrusterContributions(_shipCenterMass);
List<ThrustSet> zeroTorques = new List<ThrustSet>();
List<ThrustSet> zeroTranslations = new List<ThrustSet>();
var illegalCombos = GetIllegalCombos(_contributions);
double fuelToThrust = _itemOptions.FuelToThrustRatio;
//var test = AllCombosEnumerator(_contributions.Length, illegalCombos).ToList();
//NOTE: AllCombosEnumerator seems to only be called once, so I'm guessing AsParallel gets all enumerated values before
//dividing up the work (I would hate for it to call AllCombosEnumerator many times - once for Count(), etc)
//foreach (Tuple<ThrustSet[], ThrustSet[]> zeros in AllCombosEnumerator(_contributions.Length, illegalCombos).AsParallel().Select(o => GetZeros(_contributions, o, fuelToThrust)))
foreach (Tuple<ThrustSet[], ThrustSet[]> zeros in AllCombosEnumerator(_contributions.Length, illegalCombos).Select(o => GetZeros(_contributions, o, fuelToThrust, _shipCenterMass)))
{
if (zeros == null)
{
continue;
}
if (zeros.Item1 != null && zeros.Item1.Length > 0)
{
zeroTorques.AddRange(zeros.Item1);
}
if (zeros.Item2 != null && zeros.Item2.Length > 0)
{
zeroTranslations.AddRange(zeros.Item2);
}
}
_zeroTorqueSets = zeroTorques.ToArray();
_zeroTranslationSets = zeroTranslations.ToArray();
}
private static IEnumerable<ThrusterSetting> EnsureThrustKeysBuilt_Rotate(Vector3D axis, ThrustContribution[] contributions, double maxAcceleration, MassMatrix inertia)
{
axis = axis.ToUnit(); // doing this so the dot product can be used as a percent
List<Tuple<ThrustContribution, double>> retVal = new List<Tuple<ThrustContribution, double>>();
// Get a list of thrusters that will contribute to the direction
foreach (ThrustContribution contribution in contributions)
{
double dot = Vector3D.DotProduct(contribution.TorqueUnit, axis);
if (dot > .5d)
{
//retVal.Add(new ThrusterSetting(contribution.Thruster, contribution.Index, 1)); // for now, just do 100%
retVal.Add(Tuple.Create(contribution, contribution.TorqueLength * dot));
}
}
retVal = FilterThrusts(retVal);
#region Reduce Percent
double percent = 1;
if (retVal.Count > 0)
{
double torque = retVal.Sum(o => o.Item2);
// A=F/M
//double accel = torque / (Vector3D.DotProduct(inertia.Inertia, axis) * inertia.Mass);
double accel = torque / Math.Abs(Vector3D.DotProduct(inertia.Inertia, axis));
if (accel > maxAcceleration)
{
percent = maxAcceleration / accel;
}
}
#endregion
return retVal.Select(o => new ThrusterSetting(o.Item1.Thruster, o.Item1.Index, percent)).ToArray(); // commiting to array so that this linq isn't rerun each time it's iterated over
}
