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