/// <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);
}
}
/// <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)));
}
/// <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();
}
/// <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);
}