Commit 70f6eb1e authored by MarkG's avatar MarkG Committed by TIGERs GitLab
Browse files

Resolve "Implement multiple BestGaolKickTargets for the Ball"

Closes #1668

See merge request main/Sumatra!1421

sumatra-commit: bf67505489a3e6e86458433729974d4725b339e8
parent 39db4b67
Pipeline #17081 passed with stage
in 4 minutes and 26 seconds
......@@ -46,6 +46,8 @@ public final class AngleMath
/**
* Get the smallest difference between angle1 and angle2.<br>
* <code>norm( angle1 - angle2 )</code>
* A positive difference represents a counter clockwise turn from angle2 to angle1
* A negative difference represents a clockwise turn from angle2 to angle1
*
* @param angle1 first angle
* @param angle2 second angle
......@@ -57,6 +59,51 @@ public final class AngleMath
}
/**
* Get the resulting angle after rotating angleStart in rotationDirection for angleAmount.<br>
* Rotating with a negative value of angleAmount is equivalent to rotating the opposite direction with the absolute amount
*
* @param angleStart
* @param angleAmount
* @param rotationDirection
* @return
*/
public static double rotateAngle(final double angleStart, final double angleAmount,
final ERotationDirection rotationDirection)
{
return normalizeAngle(
rotationDirection == ERotationDirection.NO_ROTATION ? angleStart :
rotationDirection == ERotationDirection.CCW_ROTATION ?
angleStart + angleAmount :
angleStart - angleAmount
);
}
public static ERotationDirection rotationDirection(final double angleStart, final double angleEnd)
{
final var difference = AngleMath.difference(angleEnd, angleStart);
if (Math.abs(difference) < 1e-6)
{
return ERotationDirection.NO_ROTATION;
} else if (difference < 0)
{
return ERotationDirection.CW_ROTATION;
} else
{
return ERotationDirection.CCW_ROTATION;
}
}
public enum ERotationDirection
{
NO_ROTATION,
CW_ROTATION,
CCW_ROTATION
}
/**
* Get the absolute smallest difference between angle1 and angle2.<br>
* <code>abs(norm(angle1-angle2))</code>
......
......@@ -4,23 +4,23 @@
package edu.tigers.sumatra.math;
import org.junit.Test;
import static org.assertj.core.api.Assertions.assertThat;
import static org.assertj.core.api.Assertions.withinPercentage;
import static org.junit.Assert.assertEquals;
import org.junit.Test;
/**
* Testing the AngleMath module.
*
*
* @author stei_ol
*/
public class AngleMathTest
{
private static final double ACCURACY = 0.001;
private static final double ACCURACY = 0.001;
@Test
public void testNormalizeAngle()
{
......@@ -32,8 +32,8 @@ public class AngleMathTest
assertEquals(AngleMath.normalizeAngle(5f * AngleMath.PI), AngleMath.PI, ACCURACY);
assertEquals(AngleMath.normalizeAngle(4f * AngleMath.PI), 0, ACCURACY);
}
@Test
public void testTrigonometry()
{
......@@ -41,17 +41,17 @@ public class AngleMathTest
assertEquals(SumatraMath.sin(AngleMath.PI), 0, ACCURACY);
assertEquals(SumatraMath.sin(4.5f), -0.9775301, ACCURACY);
assertEquals(SumatraMath.sin(-34), -0.529, ACCURACY);
// cos
assertEquals(SumatraMath.cos(5), 0.28366, ACCURACY);
assertEquals(SumatraMath.cos(-0.1f), 0.9950, ACCURACY);
// tan
assertEquals(SumatraMath.tan(3), -0.1425, ACCURACY);
assertEquals(SumatraMath.tan(-2), 2.185, ACCURACY);
}
@Test
public void testDifference()
{
......@@ -63,8 +63,36 @@ public class AngleMathTest
assertThat(AngleMath.difference(0, 0)).isEqualTo(0.0);
assertThat(AngleMath.difference(50, 10)).isEqualTo(AngleMath.normalizeAngle(40));
}
@Test
public void testRotateAngle()
{
assertThat(AngleMath.rotateAngle(-1, 2, AngleMath.ERotationDirection.CCW_ROTATION)).isEqualTo(1);
assertThat(AngleMath.rotateAngle(1, 2, AngleMath.ERotationDirection.CW_ROTATION)).isEqualTo(-1);
assertThat(AngleMath.rotateAngle(0, 42, AngleMath.ERotationDirection.NO_ROTATION)).isEqualTo(0);
assertThat(AngleMath.rotateAngle(15 * AngleMath.PI_TWO, 42, AngleMath.ERotationDirection.NO_ROTATION))
.isEqualTo(0);
assertThat(AngleMath.rotateAngle(-1, 2 + 10 * AngleMath.PI_TWO, AngleMath.ERotationDirection.CCW_ROTATION))
.isEqualTo(1);
}
@Test
public void testRotationDirection()
{
assertThat(AngleMath.rotationDirection(-1, 1)).isEqualTo(AngleMath.ERotationDirection.CCW_ROTATION);
assertThat(AngleMath.rotationDirection(1, -1)).isEqualTo(AngleMath.ERotationDirection.CW_ROTATION);
assertThat(AngleMath.rotationDirection(1, 1)).isEqualTo(AngleMath.ERotationDirection.NO_ROTATION);
assertThat(AngleMath.rotationDirection(-1 + 10 * AngleMath.PI_TWO, 1 + 10 * AngleMath.PI_TWO))
.isEqualTo(AngleMath.ERotationDirection.CCW_ROTATION);
assertThat(AngleMath.rotationDirection(1 + 10 * AngleMath.PI_TWO, -1))
.isEqualTo(AngleMath.ERotationDirection.CW_ROTATION);
assertThat(AngleMath.rotationDirection(15 * AngleMath.PI_TWO, 10 * AngleMath.PI_TWO))
.isEqualTo(AngleMath.ERotationDirection.NO_ROTATION);
}
@Test
public void testDeg2Rad()
{
......@@ -78,8 +106,8 @@ public class AngleMathTest
assertThat(AngleMath.deg2rad(360)).isCloseTo(AngleMath.PI_TWO, withinPercentage(0.1));
assertThat(AngleMath.deg2rad(450)).isCloseTo(AngleMath.PI_TWO + AngleMath.PI_HALF, withinPercentage(0.1));
}
@Test
public void testRad2Deg()
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment