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 }