public ThrusterSolution(KeyThrustRequest request, ThrustContributionModel model, MassMatrix inertia, double mass)
{
this.Request = request;
this.Model = model;
this.Inertia = inertia;
this.Mass = mass;
}
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)
{
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, null, options);
}
_isThrustMapDirty = false;
}
private static double GetScore_Inefficient3(ThrusterMap map, ThrustContributionModel model)
{
if (map.Flattened.Length != model.Contributions.Length)
{
throw new ApplicationException("TODO: Handle mismatched map and model");
}
// Add up all the thruster forces, regardless of direction
double retVal = Enumerable.Range(0, map.Flattened.Length).
Select(o =>
{
if (map.Flattened[o].Item1 != model.Contributions[o].Item1 || map.Flattened[o].Item2 != model.Contributions[o].Item2)
{
throw new ApplicationException("TODO: Handle mismatched map and model");
}
// The translation force is the same as the thrust produced by the thruster. Don't want to include
// torque, because this method only cares about how much fuel is being used, and adding torque
// is an innacurate complication
return model.Contributions[o].Item3.TranslationForceLength * map.Flattened[o].Item3;
}).
Sum();
return retVal;
}
// This should be called twice, once for linear and once for rotation. It should add the force length if dot is close to zero. May also want a smaller length if dot is close to -1
private static double GetScore_Inefficient4(ThrusterMap map, ThrustContributionModel model, Vector3D objectiveUnit, bool isLinear)
{
const double DOT_MAX = .3; // when dot is >= this, there is no error
const double PERCENT_NEG = .5; // when dot is -1, this is the returned percent error
if (map.Flattened.Length != model.Contributions.Length)
{
throw new ApplicationException("TODO: Handle mismatched map and model");
}
double retVal = Enumerable.Range(0, map.Flattened.Length).
Select(o =>
{
if (map.Flattened[o].Item1 != model.Contributions[o].Item1 || map.Flattened[o].Item2 != model.Contributions[o].Item2)
{
throw new ApplicationException("TODO: Handle mismatched map and model");
}
// Get the actual vector
Vector3D actualVect;
double actualVal;
if (isLinear)
{
actualVect = model.Contributions[o].Item3.TranslationForceUnit;
actualVal = model.Contributions[o].Item3.TranslationForceLength;
}
else
{
actualVect = model.Contributions[o].Item3.TorqueUnit;
actualVal = model.Contributions[o].Item3.TorqueLength;
}
// Use dot to see how in line this actual vector is with the ideal
double dot = Vector3D.DotProduct(objectiveUnit, actualVect);
// Convert dot into a percent
// - If dot is near zero, error should be maximum, because this doesn't contribute to ideal
// - Can't penalize positive and negative dots equally, or it will try to find a minimum fuel useage
// that is still balanced, instead of maximum thrust that is balanced (the goal is maximum thrust
// without venting fuel out the sides)
double percent;
if (dot > 0)
{
if(dot > DOT_MAX)
{
percent = 0;
}
else
{
percent = UtilityCore.GetScaledValue(1, 0, 0, DOT_MAX, dot);
}
}
else
{
percent = UtilityCore.GetScaledValue(1, PERCENT_NEG, 0, 1, -dot);
}
return actualVal * map.Flattened[o].Item3 * percent;
}).
Sum();
return retVal;
}
private static double GetScore_Inefficient(ThrusterMap map, ThrustContributionModel model)
{
// Only focus on linear balance. Torque balance is desired
//TODO: This first version only looks a single thrusters. Make a future version that looks a more combinations
double retVal = 0;
var byThruster = model.Contributions.
ToLookup(o => o.Item1).
Where(o => o.Count() > 1);
foreach (var thruster in byThruster)
{
var arr = thruster.ToArray();
foreach (var pair in UtilityCore.GetPairs(arr.Length))
{
double dot = Vector3D.DotProduct(arr[pair.Item1].Item3.TranslationForceUnit, arr[pair.Item2].Item3.TranslationForceUnit);
if (dot < -.95)
{
// They are directly opposing each other. Figure out the force they are firing, based on the thruster percents
double force1 = arr[pair.Item1].Item3.TranslationForceLength * map.Flattened.First(o => o.Item1 == arr[pair.Item1].Item1 && o.Item2 == arr[pair.Item1].Item2).Item3;
double force2 = arr[pair.Item2].Item3.TranslationForceLength * map.Flattened.First(o => o.Item1 == arr[pair.Item2].Item1 && o.Item2 == arr[pair.Item2].Item2).Item3;
// The min represents waste (because if the weaker thruster were turned off, the net force won't change)
double min = Math.Min(force1, force2);
// Square it
retVal += min * min;
}
}
}
return retVal;
}
private static double GetScore_Inefficient2(ThrusterMap map, ThrustContributionModel model, double maxPossibleLinear, double maxPossibleRotate)
{
if (map.Flattened.Length != model.Contributions.Length)
{
throw new ApplicationException("TODO: Handle mismatched map and model");
}
// Add up all of the thruster forces. Don't worry about direction, just get the sum of the magnitude
var forces = Enumerable.Range(0, map.Flattened.Length).
Select(o =>
{
if (map.Flattened[o].Item1 != model.Contributions[o].Item1 || map.Flattened[o].Item2 != model.Contributions[o].Item2)
{
throw new ApplicationException("TODO: Handle mismatched map and model");
}
return new
{
Linear = model.Contributions[o].Item3.TranslationForceLength * map.Flattened[o].Item3,
Rotate = model.Contributions[o].Item3.TorqueLength * map.Flattened[o].Item3,
};
}).
ToArray();
double linear = forces.Sum(o => o.Linear);
double rotate = forces.Sum(o => o.Rotate);
// Subtract max possible
linear -= maxPossibleLinear;
rotate -= maxPossibleRotate;
//linear = Math.Max(linear, 0); // don't let scores go negative
//rotate = Math.Max(rotate, 0);
linear = Math.Abs(linear); // negative scores are also bad
rotate = Math.Abs(rotate);
// Return remainder squared
return (linear * linear) + (rotate * rotate);
}
public static Tuple<Vector3D, Vector3D> ApplyThrust(ThrusterMap map, ThrustContributionModel model)
{
if (map.Flattened.Length != model.Contributions.Length)
{
throw new ApplicationException("TODO: Handle mismatched map and model");
}
var combined = Enumerable.Range(0, map.Flattened.Length).
Select(o =>
{
if (map.Flattened[o].Item1 != model.Contributions[o].Item1 || map.Flattened[o].Item2 != model.Contributions[o].Item2)
{
throw new ApplicationException("TODO: Handle mismatched map and model");
}
return new
{
Index = map.Flattened[o].Item1,
SubIndex = map.Flattened[o].Item2,
Percent = map.Flattened[o].Item3,
Contribution = model.Contributions[o].Item3,
};
}).
ToArray();
Vector3D linear = Math3D.GetSum(combined.Select(o => Tuple.Create(o.Contribution.TranslationForce, o.Percent)).ToArray());
Vector3D rotation = Math3D.GetSum(combined.Select(o => Tuple.Create(o.Contribution.Torque, o.Percent)).ToArray());
return Tuple.Create(linear, rotation);
}
public static double GetMaximumPossible_Rotation(ThrustContributionModel model, Vector3D direction)
{
direction = direction.ToUnit(); // doing this so the dot product can be used as a percent
double retVal = 0d;
foreach (var contribution in model.Contributions)
{
retVal += contribution.Item3.Torque.GetProjectedVector(direction, false).Length;
}
return retVal;
}
public static double[] GetThrustMapScore3(ThrusterMap map, ThrustContributionModel model, Vector3D? linear, Vector3D? rotation, double maxPossibleLinear = 0, double maxPossibleRotate = 0)
{
if (linear == null && rotation == null)
{
throw new ApplicationException("Objective linear and rotate can't both be null");
}
Tuple<Vector3D, Vector3D> forces = ApplyThrust(map, model);
double[] retVal = new double[2];
if (forces.Item1.LengthSquared.IsNearZero() && forces.Item2.LengthSquared.IsNearZero())
{
// When there is zero contribution, give a large error. Otherwise the winning strategy is not to play :)
retVal[0] = MAXERROR; // Balance
retVal[1] = MAXERROR; // Inefficient
}
else
{
// Balance
double linearScore = GetScore_Balance(linear, forces.Item1);
double rotateScore = GetScore_Balance(rotation, forces.Item2);
retVal[0] = linearScore + rotateScore;
// Inefficient
linearScore = linear == null ? 0 : GetScore_Inefficient4(map, model, linear.Value, true);
rotateScore = rotation == null ? 0 : GetScore_Inefficient4(map, model, rotation.Value, false);
retVal[1] = linearScore + rotateScore;
}
return retVal;
}
public static SolutionError GetThrustMapScore(ThrusterMap map, ThrustContributionModel model, Vector3D? linear, Vector3D? rotation, double maxPossibleLinear = 0, double maxPossibleRotate = 0)
{
if (linear == null && rotation == null)
{
throw new ApplicationException("Objective linear and rotate can't both be null");
}
Tuple<Vector3D, Vector3D> forces = ApplyThrust(map, model);
double[] error = new double[3];
if (forces.Item1.LengthSquared.IsNearZero() && forces.Item2.LengthSquared.IsNearZero())
{
// When there is zero contribution, give a large error. Otherwise the winning strategy is not to play :)
error[0] = MAXERROR; // Balance
error[1] = MAXERROR; // UnderPowered
}
else
{
// Balance
double linearScore = GetScore_Balance(linear, forces.Item1);
double rotateScore = GetScore_Balance(rotation, forces.Item2);
error[0] = linearScore + rotateScore;
// UnderPowered
linearScore = GetScore_UnderPowered(linear, forces.Item1, maxPossibleLinear);
rotateScore = GetScore_UnderPowered(rotation, forces.Item2, maxPossibleRotate);
error[1] = linearScore + rotateScore;
}
// Inneficient
error[2] = GetScore_Inefficient(map, model);
// Total
// It's important to include the maximize thrust, but just a tiny bit. Without it, the ship will be happy to
// fire thrusters in all directions. But if maximize thrust is any stronger, the solution will have too much
// unbalance
double total = error[0] + (error[1] * .01) + (error[2] * .1);
return new SolutionError(error, total);
}
private const double MAXERROR = 100000000000; // need something excessively large, but less than double.max
#endregion
/// <summary>
/// This is a wrapper of UtilityAI.DiscoverSolution
/// </summary>
public static void DiscoverSolutionAsync(Bot bot, Vector3D? idealLinear, Vector3D? idealRotation, CancellationToken? cancel = null, ThrustContributionModel model = null, Action<ThrusterMap> newBestFound = null, Action<ThrusterMap> finalFound = null, DiscoverSolutionOptions<Tuple<int, int, double>> options = null)
{
long token = bot.Token;
// Cache Thrusters
Thruster[] allThrusters = bot.Thrusters;
if (allThrusters == null || allThrusters.Length == 0)
{
throw new ArgumentException("This bot has no thrusters");
}
// Ensure model is created (if the caller wants to find solutions for several directions at the same time, it would be
// more efficient to calculate the model once, and pass to all the solution finders)
model = model ?? new ThrustContributionModel(bot.Thrusters, bot.PhysicsBody.CenterOfMass);
// Figure out how much force can be generated in the ideal directions
double maxForceLinear = idealLinear == null ? 0d : ThrustControlUtil.GetMaximumPossible_Linear(model, idealLinear.Value);
double maxForceRotate = idealRotation == null ? 0d : ThrustControlUtil.GetMaximumPossible_Rotation(model, idealRotation.Value);
// Mutate 2% of the elements, with a 10% value drift
MutateUtility.MuateArgs mutateArgs = new MutateUtility.MuateArgs(false, .02, new MutateUtility.MuateFactorArgs(MutateUtility.FactorType.Distance, .1));
#region delegates
//NOTE: While breeding/mutating, they don't get normalized. But error calculation and returned maps need them normalized
var delegates = new DiscoverSolutionDelegates<Tuple<int, int, double>>()
{
GetNewSample = new Func<Tuple<int, int, double>[]>(() =>
{
ThrusterMap map = ThrustControlUtil.GenerateRandomMap(allThrusters, token);
return map.Flattened;
}),
GetError = new Func<Tuple<int, int, double>[], SolutionError>(o =>
{
ThrusterMap map = new ThrusterMap(ThrustControlUtil.Normalize(o), allThrusters, token);
return ThrustControlUtil.GetThrustMapScore(map, model, idealLinear, idealRotation, maxForceLinear, maxForceRotate);
}),
Mutate = new Func<Tuple<int, int, double>[], Tuple<int, int, double>[]>(o =>
{
ThrusterMap map = new ThrusterMap(o, allThrusters, token);
return ThrustControlUtil.Mutate(map, mutateArgs, false).Flattened;
}),
};
if (cancel != null)
{
delegates.Cancel = cancel.Value;
}
if (newBestFound != null)
{
delegates.NewBestFound = new Action<SolutionResult<Tuple<int, int, double>>>(o =>
{
ThrusterMap map = new ThrusterMap(ThrustControlUtil.Normalize(o.Item), allThrusters, token);
newBestFound(map);
});
}
if (finalFound != null)
{
delegates.FinalFound = new Action<SolutionResult<Tuple<int, int, double>>>(o =>
{
ThrusterMap map = new ThrusterMap(ThrustControlUtil.Normalize(o.Item), allThrusters, token);
finalFound(map);
});
}
#endregion
// Do it
//NOTE: If options.ThreadShare is set, then there's no reason to do this async, but there's no harm either
Task.Run(() => UtilityAI.DiscoverSolution(delegates, options));
}
