public void LeftIntersection_OfLowerFocusWhenSweeplineIsBelowIt_ShouldBeInOrderLeftIntersectionThenSiteThenRightIntersection
(Arc arc, Sweepline sweepline)
{
// Fixture setup
var dualArc = new Arc {
LeftNeighbour = arc.Site, Site = arc.LeftNeighbour
};
var lowerArc = arc.Site.Position.Z < dualArc.Site.Position.Z ? arc : dualArc;
var higherArc = arc.Site.Position.Z >= dualArc.Site.Position.Z ? arc : dualArc;
var directionOfLowerFocus = AngleUtilities.EquatorialDirection(lowerArc.Site.Position);
// Exercise system
var directionOfLeftIntersection = lowerArc.LeftIntersection(sweepline);
var directionOfRightIntersection = higherArc.LeftIntersection(sweepline);
// Verify outcome
var areInOrder = ArcOrderer.AreInOrder(directionOfLeftIntersection, directionOfLowerFocus, directionOfRightIntersection);
var failureString = String.Format("Direction of left intersection: {0},\n" +
"Direction of right intersection: {1},\n",
directionOfLeftIntersection, directionOfRightIntersection);
Assert.True(areInOrder, failureString);
// Teardown
}
private void OnCollisionEnter2D(Collision2D collision)
{
if (!_canTakeDamage)
{
return;
}
var magnitude = collision.relativeVelocity.magnitude;
if (magnitude < _minimumMagnitude)
{
return;
}
var angleBetween = AngleUtilities.AngleBetween(
new Vector3(_rigidBody.velocity.x, _rigidBody.velocity.y, 0),
new Vector3(collision.relativeVelocity.x, collision.relativeVelocity.y, 0));
if (angleBetween > _angleThreshold)
{
return;
}
_healthComponent.UpdateHealth(Convert.ToInt32(-magnitude * _damageModifier));
StartCoroutine(CooldownTimer());
}
public void LeftIntersection_WhenSweeplinePassesThroughLowerOfTheTwoFocii_ShouldReturnDirectionOfThatFocus
(Arc arc, Sweepline sweepline)
{
arc.LeftNeighbour.Position = Utilities.VectorAt(114, 94);
arc.Site.Position = Utilities.VectorAt(96, 77);
sweepline = Utilities.SweeplineAt(121);
// Fixture setup
var focus = arc.Site.Position;
var leftFocus = arc.LeftNeighbour.Position;
var lowerFocus = focus.Z < leftFocus.Z ? focus : leftFocus;
var directionOfLowerFocus = AngleUtilities.EquatorialDirection(lowerFocus);
sweepline.Z = lowerFocus.Z;
// Exercise system
var directionOfLeftIntersection = arc.LeftIntersection(sweepline);
// Verify outcome
var failureString = String.Format("Direction of left intersection: {0},\n",
directionOfLeftIntersection);
Assert.True(Vector.AlmostEqual(directionOfLowerFocus, directionOfLeftIntersection, Tolerance), failureString);
// Teardown
}
private void MoveToPosition(Vector3 position)
{
var targetDirection = VectorUtilities.Direction(transform.position, position);
var angleDirection = AngleUtilities.AngleDirection(transform.forward, targetDirection, transform.up);
var isBehind = AngleUtilities.AngleIsBehind(transform.forward, targetDirection);
if (isBehind && angleDirection == 0)
{
_moveComponent.MoveBack(Time.fixedDeltaTime);
return;
}
_moveComponent.MoveForward(Time.fixedDeltaTime);
switch (angleDirection)
{
case -1:
_moveComponent.TurnLeft(Time.fixedDeltaTime);
break;
case 1:
_moveComponent.TurnRight(Time.fixedDeltaTime);
break;
case 0:
break;
}
}
/// <summary>
/// calculate vmc and vmg values if possible with current state
/// </summary>
/// <param name="state">current race state</param>
public void Calculate(State state)
{
if (state.Location != null && state.TargetMark != null && state.TargetMark.Location != null && state.Course is CourseByMarks)
{
double meters = CoordinatePoint.HaversineDistance(state.Location, state.TargetMark.Location);
if (meters < _distanceCutoff)//if the reported distance is more than this threshold, it's probably garbage data
{
state.StateValues[StateValue.DistanceToTargetMarkInYards] = meters * MetersToYards;
}
var calculation = new MarkCalculation();
calculation.Location = state.Location;
calculation.Time = state.BestTime;
_previousCalculations.Add(calculation);
while (_previousCalculations.Count > _previousCalculationCount)
{
_previousCalculations.RemoveAt(0);
}
if (_previousCalculations.Count > 1)
{
var previous = _previousCalculations[_previousCalculations.Count - 2];
var duration = calculation.Time - previous.Time;
//calculate vmc
var previousDistanceMeters = CoordinatePoint.HaversineDistance(previous.Location,
state.TargetMark.Location);
var distanceDelta = previousDistanceMeters - meters;
var vmcMetersPerSecond = distanceDelta / duration.TotalSeconds;
var vmcKnots = MetersPerSecondToKnots * vmcMetersPerSecond;
calculation.VelocityMadeGoodOnCourse = vmcKnots;
state.StateValues[StateValue.VelocityMadeGoodOnCourse] = vmcKnots; //_previousCalculations.Average(x => x.VelocityMadeGoodOnCourse);
state.StateValues[StateValue.VelocityMadeGoodOnCoursePercent] = vmcKnots / state.StateValues[StateValue.SpeedInKnots] * 100;
//TODO: calculate vmg
if (state.PreviousMark != null && state.StateValues.ContainsKey(StateValue.SpeedInKnots))
{
calculation.VelocityMadeGood = VelocityMadeGood(state.TargetMark, state.PreviousMark,
calculation.Location, previous.Location, state.StateValues[StateValue.SpeedInKnots]);
state.StateValues[StateValue.VelocityMadeGoodPercent] = calculation.VelocityMadeGood.Value / state.StateValues[StateValue.SpeedInKnots] * 100;
var relativeAngle = RelativeAngleToCourse(state.TargetMark, state.PreviousMark, calculation.Location, previous.Location);
state.StateValues[StateValue.CourseOverGroundRelativeToCourse] = AngleUtilities.RadiansToDegrees(relativeAngle);
}
}
}
else if (state.Course is CourseByAngle && state.StateValues.ContainsKey(StateValue.CourseOverGroundDirection) && state.StateValues.ContainsKey(StateValue.SpeedInKnots))
{
state.StateValues[StateValue.VelocityMadeGood] = VelocityMadeGood((state.Course as CourseByAngle).CourseAngle, state.StateValues[StateValue.CourseOverGroundDirection], state.StateValues[StateValue.SpeedInKnots]);
state.StateValues[StateValue.VelocityMadeGoodPercent] = state.StateValues[StateValue.VelocityMadeGood] / state.StateValues[StateValue.SpeedInKnots] * 100;
var relativeAngle = AngleUtilities.AngleDifference(AngleUtilities.DegreestoRadians((state.Course as CourseByAngle).CourseAngle), AngleUtilities.DegreestoRadians(state.StateValues[StateValue.CourseOverGroundDirection]));
state.StateValues[StateValue.CourseOverGroundRelativeToCourse] = AngleUtilities.RadiansToDegrees(relativeAngle);
}
}
/// <summary>
/// find difference between current heading and course heading
/// </summary>
/// <param name="targetMark"></param>
/// <param name="previousMark"></param>
/// <param name="current"></param>
/// <param name="previous"></param>
/// <returns></returns>
private double RelativeAngleToCourse(Mark targetMark, Mark previousMark, CoordinatePoint current, CoordinatePoint previous)
{
if (previousMark != null && targetMark != null)
{
float courseAngle = (float)AngleUtilities.FindAngle(targetMark.Location.Project(), previousMark.Location.Project());
float boatAngle = (float)AngleUtilities.FindAngle(previous.Project(), current.Project());;
return(AngleUtilities.AngleDifference(courseAngle, boatAngle));
}
else
{
return(0);
}
}
public void Calculate(State state)
{
if (state.StateValues.ContainsKey(StateValue.ApparentWindAngle) &&
state.StateValues.ContainsKey(StateValue.ApparentWindSpeedKnots) &&
state.StateValues.ContainsKey(StateValue.SpeedInKnots) &&
(state.StateValues.ContainsKey(StateValue.CourseOverGroundDirection) ||
state.StateValues.ContainsKey(StateValue.MagneticHeading) ||
state.StateValues.ContainsKey(StateValue.MagneticHeadingWithVariation)))
{
double boatHeading = 0;
if (state.StateValues.ContainsKey(StateValue.CourseOverGroundDirection))
{
boatHeading = state.StateValues [StateValue.CourseOverGroundDirection];
}
else if (state.StateValues.ContainsKey(StateValue.MagneticHeadingWithVariation))
{
boatHeading = state.StateValues [StateValue.MagneticHeadingWithVariation];
}
else if (state.StateValues.ContainsKey(StateValue.MagneticHeading))
{
boatHeading = state.StateValues [StateValue.MagneticHeading];
}
double [] trueWindSpeed = new double[1];
double [] trueWindDirection = new double[1];
TrueWind(1
, new double[] { boatHeading }
, new double[] { state.StateValues [StateValue.SpeedInKnots] }
, new double[] { state.StateValues [StateValue.ApparentWindAngle] }
, 0.0
//TODO: change this to mag heading once it's reliable
, new double[] { boatHeading }
, new double[1]
, new double[] { state.StateValues [StateValue.ApparentWindSpeedKnots] }
, new double[] { double.MaxValue, double.MaxValue, double.MaxValue, double.MaxValue, double.MaxValue }
, trueWindDirection
, trueWindSpeed);
if (trueWindDirection[0] != double.MaxValue)
{
state.StateValues [StateValue.TrueWindAngle] = AngleUtilities.NormalizeAngleDegrees(trueWindDirection [0] - boatHeading);
state.StateValues [StateValue.TrueWindDirection] = trueWindDirection [0];
}
if (trueWindSpeed[0] != double.MaxValue)
{
state.StateValues [StateValue.TrueWindSpeedKnots] = trueWindSpeed [0];
}
}
}
public void LeftIntersection_WhenLeftNeigbourIsSameAsArcSite_ShouldReturnDirectionOfFocus
(Arc arc, Sweepline sweepline)
{
// Fixture setup
arc.LeftNeighbour = arc.Site;
var directionOfFocus = AngleUtilities.EquatorialDirection(arc.Site.Position);
// Exercise system
var directionOfLeftIntersection = arc.LeftIntersection(sweepline);
// Verify outcome
var failureString = String.Format("Direction of left intersection: {0},\n",
directionOfLeftIntersection);
Assert.True(Vector.AlmostEqual(directionOfFocus, directionOfLeftIntersection, Tolerance), failureString);
// Teardown
}
public void EquatorialMidpoint_OfTwoNonpolarVectors_ShouldBeInOrderWithTheTwoVectorsEquatorialDirections
(Vector3 u, Vector3 v)
{
// Fixture setup
var directionOfU = AngleUtilities.EquatorialDirection(u);
var directionOfV = AngleUtilities.EquatorialDirection(v);
// Exercise system
var midpoint = AngleUtilities.EquatorialMidpoint(u, v);
// Verify outcome
var areInOrder = ArcOrderer.AreInOrder(directionOfU, midpoint, directionOfV);
var failureString = String.Format("Midpoint was {0}", midpoint);
Assert.True(areInOrder, failureString);
// Teardown
}
/// <summary>
/// compare the new state to the last values and determine if a tack has occured
/// if so, update the state with the new tack
/// </summary>
/// <param name="state"></param>
private void CheckForTack(State state)
{
var latest = _history.Last();
var deltas = _history.Where(x => x.Time > latest.Time - _tackThresholdTime).Select(x => Math.Abs(AngleUtilities.AngleDifference(latest.CourseOverGroundRadians, x.CourseOverGroundRadians))).Max();
if (deltas > _tackThreshold)
{
//tack detected
_lastTackAt = latest.Time;
var priorToTack = _history.Where(x => x.Time < latest.Time - _dataExclusionTime).OrderByDescending(x => x.Time).FirstOrDefault();
if (priorToTack != null)
{
_previousTackCourseOverGroundRadians = priorToTack.CourseOverGroundRadians;
}
else
{
_previousTackCourseOverGroundRadians = null;
}
_history.Clear();
_currentTackStartCourseOverGroundRadians = null;
var difference = AngleUtilities.AngleDifference(_previousTackCourseOverGroundRadians.Value, latest.CourseOverGroundRadians);
var differenceDegrees = AngleUtilities.RadiansToDegrees(difference);
string message = string.Format("Tack: {0:0.0}°", differenceDegrees);
_logger.Info(message);
state.AddMessage(MessageCategory.Tactical, MessagePriority.Normal, 5, message);
//record the tack in the state
if (state.RaceStarted && _currentTack != null)
{
_currentTack.CourseOverGround = AngleUtilities.RadiansToDegrees(_previousTackCourseOverGroundRadians.Value);
state.Tacks.Add(_currentTack);
}
_currentTack = new Tack();
_currentTack.At = latest.Time;
}
}
public static void VmgTest()
{
double courseAngle = 270;
double courseOverGround = 270 - 45;
double speed = 10;
double courseAngleRadians = AngleUtilities.DegreestoRadians(courseAngle);
double courseOverGroundRadians = AngleUtilities.DegreestoRadians(courseOverGround);
double difference = AngleUtilities.AngleDifference(courseAngleRadians, courseOverGroundRadians);
var cos = Math.Cos(difference);
var vmg = cos * speed;
vmg = MarkCalculator.VelocityMadeGood(courseAngle, courseOverGround, speed);
Console.WriteLine(string.Format("VMG:{0:0.00}\tVMG%:{1:0.00}", vmg, (vmg / speed * 100)));
}
public void EquatorialMidpoint_OfTwoNonpolarVectors_ShouldBeEquidistantFromThoseVectorsEquatorialDirections
(Vector3 u, Vector3 v)
{
// Fixture setup
var directionOfU = AngleUtilities.EquatorialDirection(u);
var directionOfV = AngleUtilities.EquatorialDirection(v);
// Exercise system
var midpoint = AngleUtilities.EquatorialMidpoint(u, v);
// Verify outcome
var distanceToU = Trig.InverseCosine(directionOfU.ScalarMultiply(midpoint));
var distanceToV = Trig.InverseCosine(directionOfV.ScalarMultiply(midpoint));
Debug.WriteLine(distanceToU + "," + distanceToV);
var failureString = String.Format("Midpoint was {0}", midpoint);
Assert.True(Number.AlmostEqual(distanceToU, distanceToV, Tolerance), failureString);
// Teardown
}
/// <inheritdoc />
public void Calculate(State state)
{
if (state.StateValues.ContainsKey(StateValue.CourseOverGroundDirection))
{
var cogRads = AngleUtilities.DegreestoRadians(state.StateValues[StateValue.CourseOverGroundDirection]);
//make sure whe're not in an "exclusion" aka a few seconds before/after a known tack
if (!_lastTackAt.HasValue || (_lastTackAt.Value + _dataExclusionTime < state.BestTime))
{
if (!_currentTackStartCourseOverGroundRadians.HasValue)
{
_currentTackStartCourseOverGroundRadians = cogRads;
}
_history.Add(new CourseHistory()
{
Time = state.BestTime, CourseOverGroundRadians = cogRads
});
//make sure we have enough data to do the calculation accurately
if (_history.Count > 1)
{
if (_history.Max(x => x.Time) - _history.Min(x => x.Time) > _tackThresholdTime)
{
CheckForTack(state);
}
}
}
//calculate the delta on the current tack
if (state.StateValues.ContainsKey(StateValue.CourseOverGroundDirection) && _currentTackStartCourseOverGroundRadians.HasValue)
{
var delta = AngleUtilities.AngleDifference(cogRads, _currentTackStartCourseOverGroundRadians.Value);
state.StateValues[StateValue.CurrentTackCourseOverGroundDelta] = AngleUtilities.RadiansToDegrees(delta);
}
}
PurgeOldHistory();
}
public void LeftIntersection_WhenSweeplinePassesThroughBothFocuses_ShouldReturnEquatorialMidpointOfFocii
(Arc arc, Sweepline sweepline)
{
// Fixture setup
var colatitudeOfFocus = arc.Site.Position.SphericalCoordinates().Colatitude;
var azimuthOfFocus = arc.Site.Position.SphericalCoordinates().Azimuth;
var azimuthOfLeftFocus = arc.LeftNeighbour.Position.SphericalCoordinates().Azimuth;
arc.Site.Position = new SphericalCoords(colatitudeOfFocus, azimuthOfFocus).CartesianCoordinates();
arc.LeftNeighbour.Position = new SphericalCoords(colatitudeOfFocus, azimuthOfLeftFocus).CartesianCoordinates();
// Exercise system
var directionOfLeftIntersection = arc.LeftIntersection(sweepline);
// Verify outcome
var equatorialMidpoint = AngleUtilities.EquatorialMidpoint(arc.LeftNeighbour.Position, arc.Site.Position);
var failureString = String.Format("Direction of left intersection: {0},\n",
directionOfLeftIntersection);
Assert.True(Vector.AlmostEqual(equatorialMidpoint, directionOfLeftIntersection, Tolerance), failureString);
// Teardown
}
/// <summary>
/// calculate vmg from raw values
/// </summary>
/// <param name="courseAngle"></param>
/// <param name="courseOverGround"></param>
/// <param name="speed"></param>
/// <returns></returns>
public static double VelocityMadeGood(double courseAngle, double courseOverGround, double speed)
{
return(Math.Cos(Math.Abs(AngleUtilities.AngleDifference(AngleUtilities.DegreestoRadians(courseAngle), AngleUtilities.DegreestoRadians(courseOverGround)))) * speed);
}
