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 void Update_AnyThread(double elapsedTime)
{
Vector3D velocity = this.Bot.VelocityWorld.ToUnit();
double dot = Vector3D.DotProduct((Vector3D)_velocityUnitLastTick, velocity);
_velocityUnitLastTick = velocity;
double score;
if (Math1D.IsInvalid(dot))
{
score = .5d; // not sure what to do here
}
else if (dot < _minDot)
{
//score = UtilityHelper.GetScaledValue(0, 1, -1, _minDot, dot);
score = 0d; // this class is pretty high scoring as it is, so I don't really want a gradient
}
else
{
score = 1d; // I don't want a gradient above this min. Circular motion will have a dot of zero, and shouldn't be favored or punished over straight line motion
}
_score = (double)_score + (score * elapsedTime);
}
public static bool IsInvalid(this VectorND vector)
{
double[] arr = vector.VectorArray;
if (arr == null)
{
return(false); // it could be argued that this is invalid, but all the other types that have IsInvalid only consider the values that Math1D.IsInvalid looks at
}
foreach (double value in arr)
{
if (Math1D.IsInvalid(value))
{
return(true);
}
}
return(false);
}
private void InvalidateGeometry()
{
Vector3D direction = _toPoint - _fromPoint;
double directionLength = direction.Length;
if (Math1D.IsInvalid(directionLength) || Math1D.IsNearZero(directionLength))
{
_lineModel.Transform = new ScaleTransform3D(0, 0, 0);
if (_fromArrowModel != null)
{
_fromArrowModel.Transform = new ScaleTransform3D(0, 0, 0);
}
if (_toArrowModel != null)
{
_toArrowModel.Transform = new ScaleTransform3D(0, 0, 0);
}
return;
}
Vector3D directionUnit = new Vector3D();
double arrowWidth = 0;
double arrowLength = 0;
double halfArrowLength = 0;
if ((_fromArrowModel != null && _fromArrowPercent != null) || (_toArrowModel != null && _toArrowPercent != null)) // they should both be null or not null together, just being safe
{
directionUnit = direction.ToUnit();
arrowWidth = _thickness * _arrowBaseMult * _arrowSizeMult;
arrowLength = _thickness * _arrowHeightMult * _arrowSizeMult;
halfArrowLength = arrowLength / 2;
}
#region line
double length = directionLength;
// Reduce the length if the arrow is at the end so that the line graphic doesn't poke through the arrow graphic
double?fromOffset = null;
if (_fromArrowModel != null && _fromArrowPercent != null && _fromArrowPercent.Value.IsNearValue(1))
{
fromOffset = halfArrowLength;
length -= halfArrowLength;
}
if (_toArrowModel != null && _toArrowPercent != null && _toArrowPercent.Value.IsNearValue(1))
{
length -= halfArrowLength;
}
Transform3DGroup transform = new Transform3DGroup();
if (length > Math1D.NEARZERO)
{
transform.Children.Add(new ScaleTransform3D(_thickness, _thickness, length));
if (fromOffset != null)
{
transform.Children.Add(new TranslateTransform3D(0, 0, fromOffset.Value));
}
transform.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(_initialOrientation, StaticRandom.NextDouble(360d)))); // spin
transform.Children.Add(new RotateTransform3D(new QuaternionRotation3D(Math3D.GetRotation(_initialOrientation, direction))));
transform.Children.Add(new TranslateTransform3D(_fromPoint.ToVector()));
}
else
{
// The arrows are visible, but there's not enough length to show the line graphic
transform.Children.Add(new ScaleTransform3D(0, 0, 0));
}
_lineModel.Transform = transform;
#endregion
#region from arrow
if (_fromArrowModel != null && _fromArrowPercent != null)
{
Vector3D arrowOffset = directionUnit * arrowLength;
Point3D arrowPosition = _fromPoint + (direction * (1d - _fromArrowPercent.Value));
arrowPosition += arrowOffset; // this is because the tip of the arrow should be that the percent, not the base
transform = new Transform3DGroup();
transform.Children.Add(new ScaleTransform3D(arrowWidth, arrowWidth, arrowLength));
transform.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(_initialOrientation, StaticRandom.NextDouble(360d)))); // spin
transform.Children.Add(new RotateTransform3D(new QuaternionRotation3D(Math3D.GetRotation(_initialOrientation, -direction))));
transform.Children.Add(new TranslateTransform3D(arrowPosition.ToVector()));
_fromArrowModel.Transform = transform;
}
#endregion
#region to arrow
if (_toArrowModel != null && _toArrowPercent != null)
{
Vector3D arrowOffset = directionUnit * arrowLength;
Point3D arrowPosition = _fromPoint + (direction * _toArrowPercent.Value);
arrowPosition -= arrowOffset; // this is because the tip of the arrow should be that the percent, not the base
transform = new Transform3DGroup();
transform.Children.Add(new ScaleTransform3D(arrowWidth, arrowWidth, arrowLength));
transform.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(_initialOrientation, StaticRandom.NextDouble(360d)))); // spin
transform.Children.Add(new RotateTransform3D(new QuaternionRotation3D(Math3D.GetRotation(_initialOrientation, direction))));
transform.Children.Add(new TranslateTransform3D(arrowPosition.ToVector()));
_toArrowModel.Transform = transform;
}
#endregion
}